REPORT  DOCUMENTATION  PAGE 


Form  Approved 
OMB  No.  074-0188 


i 


Public  reporting  burden  for  this  collection  of  information  is  estimated  to  average  1  hour  per  response,  including  g  the  time  for  reviewing  instructions,  searching  existing  data  sources, 
gathering  and  maintaining  the  data  needed,  and  completing  and  reviewing  the  collection  of  information.  Send  comments  regarding  this  burden  estimate  or  any  other  aspect  of  the 
collection  of  information,  including  suggestions  for  reducing  this  burden  to  Washington  Headquarters  Services,  Directorate  for  Information  Operations  and  Reports,  1215  Jefferson 

Davis  Highway,  Suite  1204,  Arlington,  VA  22202-4302,  and  to  the  Office  of  Management  and  Budget,  Paperwork  Reduction  Project  (0704-0188),  Washington,  DC  20503. 

1.  AGENCY  USE  ONLY  (Leave  blank) 

2.  REPORT  DATE 

18  May  2005 

3.  REPORT  TYPE  AND  DATE  COVERED 

4.  TITLE  AND  SUBTITLE 

Artificial  potential  field  controllers  for  robust 
communications  in  a  network  of  swarm  robots 

5.  FUNDING  NUMBERS 

6.  AUTHOR(S) 

Dunbar,  Thomas  W.  (Thomas  Ward) ,  1982- 

7.  PERFORMING  ORGANIZATION  NAME(S)  AND  ADDRESS(ES) 

8.  PERFORMING  ORGANIZATION  REPORT  NUMBER 

9.  SPONSORING/MONITORING  AGENCY  NAME(S)  AND  ADDRESS(ES) 

10.  SPONSORING/MONITORING  AGENCY  REPORT  NUMBER 

US  Naval  Academy 
Annapolis,  MD  21402 

Trident  Scholar  project  report  no. 

335  (2005) 

11.  SUPPLEMENTARY  NOTES 

12a.  DISTRIBUTION/AVAILABILITY  STATEMENT 

This  document  has  been  approved  for  public  release;  its  distribution 
is  UNLIMITED. 

12b.  DISTRIBUTION  CODE 

13  •  ABSTRACT:  An  active  area  of  research  in  the  robotics  community  is  “swarm  control,”  where  many  simple  robots  work  together  to  execute  tasks  which  are  beyond 
the  capability  of  any  single  robot  acting  alone.  Yet  in  order  for  the  swarm  members  to  work  together  effectively  they  must  maintain  a  reliable  and  robust  wireless 
communication  network  among  themselves.  The  goal  of  this  project  was  to  create  a  motion  control  law  which  could  fulfill  the  dual  and  sometimes  conflicting  requirements  of 
executing  a  primary  mission  (e.g.,  search  and  rescue),  while  maintaining  a  robust  mobile  wireless  communication  network  among  the  swarm  members.  The  success  or  failure  in 
sending  or  receiving  a  wireless  message  is  inherently  probabilistic,  but  the  odds  of  successfully  relaying  a  message  increase  considerably  based  upon  the  spatial  arrangement  of 
the  swarm  members.  This  imposes  a  variety  of  constraints  on  each  robot’s  motion.  Each  robot  sending  a  message  should:  1 .  maintain  a  line  of  sight  to  the  receiving  robot  (esp. 
in  an  environment  like  a  cave  or  bunker  with  dense  walls);  2.  stay  within  close  proximity  of  the  receiving  robot  (the  range  is  dictated  by  the  power  of  the  transmitter);  and  3. 
increase  the  overall  redundancy  of  the  swarm  by  maintaining  requirements  1  and  2  for  two  or  more  receiving  robots  simultaneously.  To  this  end,  several  artificial  potential  field 
controllers  -  a  popular  method  of  robotic  control  -  have  been  developed  in  this  project  and  simulated  to  determine  their  success  in  controlling  the  swarm.  At  a  higher  level,  the 
project  addressed  the  challenge  of  composing  a  motion  control  law  to  achieve  the  primary  mission,  while  maintaining  as  many  communication  constraints  as  possible.  This 
project  included  a  proof-of-concept  implementation  of  the  motion  control  law  on  real  robots.  In  addition,  this  project  simulated  and  statistically  analyzed  the  controller  to 
determine  its  effectiveness  at  achieving  the  primary  mission  and  maintaining  a  robust  communication  network.  The  effectiveness  of  the  control  law  was  seen  both  in  simulation 
and  experiment.  Overall  the  robustness  of  the  swarm  was  increased  200-300%  in  the  scenarios  considered. 

14.  SUBJECT  TERMS: 

Swarm  robotics,  Communication  Networks,  Artificial  potential  fields,  Line  of 
Sight,  Redundancy 

15.  NUMBER  OF  PAGES 

127 

16.  PRICE  CODE 

17.  SECURITY  CLASSIFICATION 

OF  REPORT 

18.  SECURITY  CLASSIFICATION 

OF  THIS  PAGE 

19.  SECURITY  CLASSIFICATION 

OF  ABSTRACT 

20.  LIMITATION  OF  ABSTRACT 

NSN  7540-01  -280-5500  Standard  Form  298 

(Rev. 2-89)  Prescribed  by  ANSI  Std.  Z39-1 8 

298-102 


1 


Abstract 

An  active  area  of  research  in  the  robotics  community  is  “swarm  control,”  where  many  simple  robots  work 
together  to  execute  tasks  which  are  beyond  the  capability  of  any  single  robot  acting  alone.  Yet  in  order  for  the 
swarm  members  to  work  together  effectively  they  must  maintain  a  reliable  and  robust  wireless  communication 
network  among  themselves. 

The  goal  of  this  project  was  to  create  a  motion  control  law  which  could  fulfill  the  dual  and  sometimes 
conflicting  requirements  of  executing  a  primary  mission  (e.g.,  search  and  rescue),  while  maintaining  a  robust 
mobile  wireless  communication  network  among  the  swarm  members. 

The  success  or  failure  in  sending  or  receiving  a  wireless  message  is  inherently  probabilistic,  but  the  odds 
of  successfully  relaying  a  message  increase  considerably  based  upon  the  spatial  arrangement  of  the  swarm 
members.  This  imposes  a  variety  of  constraints  on  each  robot’s  motion.  Each  robot  sending  a  message 
should: 

1.  maintain  a  line  of  sight  to  the  receiving  robot  (esp.  in  an  environment  like  a  cave  or  bunker  with  dense 
walls); 

2.  stay  within  close  proximity  of  the  receiving  robot  (the  range  is  dictated  by  the  power  of  the  transmitter); 
and 

3.  increase  the  overall  redundancy  of  the  swarm  by  maintaining  requirements  1  and  2  for  two  or  more 
receiving  robots  simultaneously. 

To  this  end,  several  artificial  potential  field  controllers  -  a  popular  method  of  robotic  control  -  have  been 
developed  in  this  project  and  simulated  to  determine  their  success  in  controlling  the  swarm.  At  a  higher 
level,  the  project  addressed  the  challenge  of  composing  a  motion  control  law  to  achieve  the  primary  mission, 
while  maintaining  as  many  communication  constraints  as  possible. 

This  project  included  a  proof-of-concept  implementation  of  the  motion  control  law  on  real  robots.  In 
addition,  this  project  simulated  and  statistically  analyzed  the  controller  to  determine  its  effectiveness  at 
achieving  the  primary  mission  and  maintaining  a  robust  communication  network.  The  effectiveness  of  the 
control  law  was  seen  both  in  simulation  and  experiment.  Overall  the  robustness  of  the  swarm  was  increased 
200-300%  in  the  scenarios  considered. 

Keywords:  Swarm  robotics,  Communication  Networks,  Artificial  potential  fields,  Line  of  Sight,  Redun¬ 
dancy 
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Chapter  1 

Introduction 


A  swarm  of  robots  consists  of  many  small,  cheap,  unsophisticated  robots  working  together  to  accomplish  a 
task  more  efficiently  than  a  single  robot.  There  are  many  potential  applications  for  swarm  robotics,  some 
military  applications  include,  but  are  not  limited  to,  mine  countermeasures  both  in  water  and  on  land  [25]. 
Swarms  are  ideal  for  such  applications  because  a  swarm  of  robots  can  spread  out  and  effectively  cover  a 
large  area  in  a  short  time.  In  addition,  if  one  robot  is  destroyed  by  a  mine,  the  rest  of  the  swarm  continues 
to  function.  For  these  reasons,  swarms  are  also  excellent  for  search  and  rescue  or  reconnaissance  operations 
(e.g.  working  in  the  caves  of  Afghanistan)  [40].  For  example,  a  search  and  rescue  swarm  [34]  could  be  used 
in  a  disaster  area  such  as  the  debris  of  the  World  Trade  Center,  the  aftermath  of  an  earthquake,  searching 
through  a  darkened,  stricken  vessel,  or  a  burning  building. 

Swarms  of  robots  have  several  advantages  over  a  single  robot  or  even  a  small  group  of  complex  robots: 

•  the  individual  units  of  a  swarm  are  usually  less  complex  than  a  single  robot,  meaning  they  are  cheaply 
and  easily  mass  produced; 

•  a  swarm  is  easier  to  reconfigure  for  new  missions; 

•  swarms  are  more  fault  tolerant  than  a  single  robot  because  if  one  unit  fails  the  rest  of  the  swarm  still 
functions;  and 

•  the  control  of  the  robots  is  decentralized  so  there  is  no  “master”  robot  that  controls  the  rest  of  the 
swarm,  allowing  well  designed  control  and  communication  algorithms  to  scale  linearly  as  the  size  of 
the  swarm  increases. 

Swarms  are  common  in  biological  systems.  Bees  communicate  and  work  together  to  defend  the  hive  and 
produce  honey.  Ants,  when  working  collectively,  can  move  objects  several  times  their  mass.  Wolves  use  packs 
to  hunt  and  fish  form  schools  for  protection  and  hydrodynamic  efficiency.  When  robots  work  together  in  a 
swarm  they  display  some  of  the  same  behaviors  and  accomplishments  that  co-operative  insects  and  animals 
demonstrate. 

Yet  in  order  for  the  swarms  to  work  together  effectively  they  must  have  reliable  and  robust  communication 
among  the  team  members.  This  project  focuses  on  the  theoretical  design  and  experimental  analysis  of  a 
distributed  motion  control  methodology  for  communicating  swarms  of  robots. 


1  Problem  statement 

The  success  or  failure  in  sending  or  receiving  a  wireless  message  between  robots  is  inherently  probabilistic. 
One  way  to  increase  the  odds  of  successfully  relaying  a  message  without  increasing  the  power  required  is  to 


carefully  construct  the  spatial  arrangement  of  the  swarm  members  to  facilitate  communication.  This  imposes 
a  variety  of  constraints  on  each  robot’s  motion;  each  robot  sending  a  message  should  (see  Figure  1.1): 

1.  maintain  a  line  of  sight  to  a  receiving  robot  (especially  in  an  environment  like  a  cave  or  bunker  with 
dense  walls);  and 

2.  stay  within  close  proximity  of  the  receiving  robot  (the  range  is  dictated  by  the  power  of  the  transmitter). 

Furthermore,  a  good  control  methodology  will  exploit  the  redundancy  of  the  swarm  to  improve  robustness 
to  robot  failure.  To  that  end  a  third  requirement  can  be  introduced  (refer  again  to  Figure  1.1): 

3.  The  robots  can  be  constrained  to  move  so  that  each  robot  is  communicating  (i.e.  meeting  objectives  1 
and  2)  with  at  least  two  other  units  at  all  times. 

Therefore  if  one  unit  fails,  the  other  robots  are  still  communicating  with  at  least  one  other  swarm  member. 
This  ensures  that  if  a  single  unit  fails,  no  one  unit  becomes  isolated  from  all  other  units. 


Figure  1.1:  The  test  scenario.  Maintaining  multiple  connections  per  robot,  as  well  as  line  of  sight  and  range 
constraints  enables  robust  communication. 

Therefore,  the  overall  design  goal  of  this  Trident  Project  is  to  create  a  swarm  motion  control  algorithm. 
The  algorithm  must  meet  the  following  criteria. 

1.  Goal  Completion  Compute  robot  velocities  to  allow  the  swarm  to  collectively  accomplish  a  primary 
mission  such  as  search  and  rescue  or  exploration. 

2.  Robust  Communication  Ensure  that  each  individual  robot  maintains  line  of  sight  and  proximity  with 
at  least  two  other  robots  at  all  times  (see  Constraints  1-3  above). 

3.  Distributed  Operation  Each  robot  makes  its  own  motion  control  decisions  based  on  locally  available 
information  and  no  single  robot  acts  as  the  leader  or  “master”  robot.  A  well  designed  distributed 
control  algorithm  would  be  able  to  scale  linearly  with  the  number  of  robots  in  the  swarm. 
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4.  Fault  Tolerant  Do  not  allow  the  failure  of  one  robot  to  be  detrimental  to  the  swarm  performing  any  of 
its  missions 

The  above  criteria  are  important  because  they  reflect  the  swarm  philosophy  of  efficiency  and  robustness, 
discussed  earlier.  However,  this  poses  a  significant  theoretical  challenge  because  Criteria  1  -.Goal  Completion 
and  2 : Robust  Communication  may  at  times  be  at  odds  with  each  other.  Furthermore,  when  the  algorithm 
is  distributed  according  to  the  Criteria  3 :  Distributed  Operation,  there  is  no  central  “master”  robot  to  break 
such  deadlocks.  Only  a  carefully  designed  motion  control  algorithm  can  address  all  of  these  criteria. 

2  Related  work  to  problem  statement 

A  swarm  can  be  defined  as  a  group  of  robots  moving  in  some  collective  coordinated  fashion  without  any 
rigidly  defined  relative  spatial  arrangement  between  robots.  The  first  work  on  “swarms”  was  provided  by 
Reynolds  [32] .  This  work  attempted  to  devise  a  set  of  motion  laws  for  a  group  of  agents  so  that  the  collective 
motion  would  mimic  that  of  biological  system  such  as  schools  of  fish  and  flocking  birds.  The  resulting 
motion  is  called  flocking.  This  work  was  primarily  targeted  at  computer  graphics  applications.  It  was  not 
considered  appropriate  for  robotics  because  it  did  not  display  robust  behavior  in  the  face  of  unstructured 
or  changing  environments.  Little  work  appeared  in  the  field  until  Swaroop  and  Hedrick  [35]  among  others 
([10,  27])  began  considering  robot  formations,  primarily  inspired  by  the  automated  highway  project  and 
military  applications.  In  contrast  to  swarms,  formation  applications  require  that  a  group  of  robots  maintain 
a  fixed  relative  spatial  orientation,  despite  movement  of  the  group  as  a  whole.  In  addition,  some  members 
of  the  group  are  arbitrarily  selected  as  the  “lead”  robots,  making  the  control  scheme  somewhat  centralized 
rather  than  distributed,  violating  Criteria  3 : Distributed  Operation. 

More  recently  there  has  been  increased  interest  in  swarm  applications.  As  a  result,  the  work  of  Reynolds 
has  inspired  a  new  round  of  developments  in  the  area  of  flocking.  Flocking  implies  that,  while  the  entire 
group  of  robots  does  not  need  to  maintain  a  fixed  relative  spatial  orientation  as  in  formations,  the  entire 
group  is  required  to  possess  identical  velocity  vectors  at  steady  state  ([26,  38,  18]).  The  primary  theoretical 
challenge  in  this  work  is  to  show  collective  stability  (i.e.  that  the  velocity  of  the  members  converges  to  some 
constant)  and  ensuring  that  the  members  do  not  collide.  In  the  works  cited  in  this  paragraph  it  is  shown 
that  either  all-to-all  member  information  sharing  is  required  or  some  centralized  signal  must  be  broadcast 
to  guarantee  stability;  again  this  violates  Criteria  3 :  Distributed  Operation. 

Formation  and  flocking  approaches  for  motion  control  are  not  appropriate  for  this  project  because  they 
do  not  afford  robots  the  ability  to  operate  as  independent  units  and  therefore  may  not  be  appropriate  for 
all  types  of  missions.  Approaches  requiring  centralized  information  are  not  truly  distributed  and  will  not 
scale  well  (Criteria  3 '.Distributed,  Operation) .  Requiring  a  leader  makes  the  swarm  susceptible  to  single  robot 
failure  and  thus  this  approach  also  fails  Criteria  4 -.Fault  Tolerance. 

The  issue  of  communication  in  mobile  networks  is  frequently  considered  in  wireless  communication  and 
mobile  computing  literature.  For  example,  Rus  [33]  and  Olfati-Saber  and  Murray  [28]  consider  routing  and 
consensus  problems  in  networks  with  changing  topologies.  However,  this  work  assumes  the  motion  of  the 
network  nodes  is  not  controllable  and  therefore  they  do  not  address  Criteria  2 : Robust  Communications.  Li 
and  Rus  [21]  consider  networking  where  the  motions  of  the  nodes  is  controllable,  but  only  seek  to  create  an 
ad-hoc  network  and  not  a  robust  communications  network,  again  failing  Criteria  2 -.Robust  communication. 
Similarly  Bishop  [6]  and  Liu  and  Passino  [22]  consider  a  swarm  control  method  that  does  permit  deviations 
in  individual  motions  but  only  addresses  collective  qualities  of  interest  and  cannot  enforce  Criteria  2 :  Robust 
Communications. 


3  Project  goals 

The  goals  of  this  project  are  as  follows. 
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Theoretical  goals 

•  a  distributed  control  law  for  maintaining  an  appropriate  separation  distance  between  two  robots; 

•  a  distributed  control  law  for  maintaining  line  of  sight  between  two  robots; 

•  a  distributed  control  law  for  enforcing  the  redundancy  constraint  (i.e.  maintaining  two  or  more  con¬ 
nections  per  robot);  and 

•  a  novel  algorithm  for  composing  the  above  three  controllers  along  with  the  primary  mission  control 
law,  in  such  a  way  that  a  maximum  number  of  objectives  are  achieved. 

Implementation  and  design  goals 

•  a  “proof  of  concept”  demonstration  of  the  operation  of  the  swarm  meeting  the  three  design  criteria 
outlined  in  Section  1,  using  3  robots. 

Experimental/Simulation  analysis  goals 

•  simulation  of  a  large  number  of  robots  moving  according  to  the  control  law; 

•  quantitative  analysis  of  swarm  performance  in  completing  both  the  primary  mission  and  connectivity 
and  robustness  in  a  swarm  communication  network. 

The  next  section  outlines  specific  tasks  that  were  accomplished  to  successfully  realize  the  goals  and 
explains  the  overall  operation  of  the  robot  and  interaction  of  experimental  components. 


4  Overview  of  project 

Figure  1.3  shows  the  hardware  involved.  Up  to  8  Koala  robots  (Figure  1.2),  each  with  a  on-board  PC, 
separate  Motorola  microprocessor,  and  a  radio  Ethernet  attachment  are  placed  in  an  environment  populated 
with  obstacles.  In  a  military  application,  each  of  the  robots  would  have  a  GPS  receiver  to  give  absolute 
position  and  a  stereo  vision  setup  or  360°  laser  range  finder  to  determine  the  position  of  obstacles  and  other 
robots.  Because  that  equipment  is  not  available  for  this  project  and  because  the  self  localization  problem  is 
far  beyond  the  scope  of  this  work,  an  overhead  camera  interfaced  to  a  base  station  PC  will  act  as  an  “eye 
in  the  sky”  transmitting  position  information  to  the  robots.  This  is  a  standard  technique  in  mobile  robotics 
research  and  is  analogous  to  having  a  satellite  viewing  several  robotic  tanks  and  broadcasting  the  tanks’ 
position. 


Figure  1.2:  The  Koala  Robot. 
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Figure  1.3:  The  overall  experimental  setup. 
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Figure  1.4  outlines  the  operation  of  the  system.  One  “cycle”  of  the  system’s  operation  begins  with  an 
overhead  digital  camera  taking  a  snapshot  of  the  scene.  The  raw  image  must  be  imported  to  the  base  station 
PC  using  the  MATLAB  Image  Acquisition  Toolbox.  The  raw  image  is  then  processed  using  programs  written 
in  the  MATLAB  high  level  mathematical  programming  language  (described  in  Chapter  4  Section  1.1).  The 
positions  and  orientations  of  each  robot,  as  well  as  the  coordinates  of  the  vertices  of  each  polygonal  obstacle 
are  extracted  from  the  image.  This  position  information  is  used  by  five  separate  motion  control  functions: 
Range,  Line  of  Sight,  Redundancy,  Obstacle  Avoidance  and  Go-To-Goal.  Some  background  on  the  motion 
control  technique  used  in  this  work  appears  in  Chapter  2  while  the  derivation  of  the  three  novel  controllers, 
Range,  Line  of  Sight  and  Redundancy,  is  detailed  in  Chapter  3.  Each  of  these  functions  returns  a  suggested 
body  velocity  for  the  robot  to  achieve  the  respective  objective.  Each  of  these  velocities  is  an  input  to  the 
parallel  composition  algorithm  whose  function  is  to  compose  these  desired  motion  directions  so  that  the  final 
desired  body  velocity  accomplishes  as  many  of  these  objectives  as  possible.  The  algorithm  is  described  in 
detail  in  Chapter  3  Section  4.  The  final  output  velocity  of  the  controller  is  then  transformed  into  a  left  and 
right  wheel  velocity  (see  Chapter  2  Section  3) .  These  wheel  velocities  are  written  to  a  file  which  is  sent  over 
the  wireless  network  to  the  hard  drive  of  the  appropriate  robot  (Chapter  4  Section  2). 

At  each  cycle,  the  individual  robots  check  their  hard  drives  for  updates.  If  a  new  velocity  file  is  found, 
it  is  read.  The  robot  then  writes  these  velocity  values  over  the  serial  line  to  the  embedded  Motorola  68K 
microprocessor  which  sets  the  motor  input  voltages  accordingly  (see  Chapter  4  Section  3). 

The  input  voltages  then  cause  the  robots  to  move.  The  overhead  camera  takes  another  picture  and  the 
process  repeats  indefinitely. 

A  second  phase  of  the  project  was  to  perform  experiments  at  different  desired  numbers  of  communication 
links  per  robot.  Simulations  proved  to  be  the  most  effective  way  to  perform  hundreds  of  tests  of  the  controller 
in  many  situations  not  easily  recreated  in  the  lab.  Chapter  5  Section  3  explains  the  experimental  procedure 
used  for  this  phase  of  the  project.  Additionally,  the  results  of  these  experiments  were  quantitatively  analyzed 
(see  Chapter  5  Section  2)  to  determine  the  percentage  increase  in  the  robustness  of  the  communication 
network  as  compared  to  a  baseline  controller  (Chapter  5  Section  4) . 

4.1  Organization  of  the  report 

In  Chapter  2,  some  common  techniques  of  robot  motion  control  are  reviewed.  While  many  of  these  are 
standard,  the  implementation  was  project-specific  and  a  necessary  step  toward  the  end-state. 

In  Chapter  3  the  primary  theoretical  contributions  of  the  project  are  reported.  The  Range,  Line  of  Sight 
and  Redundancy  control  laws  are  derived  and  verified  through  computer  simulation.  Finally,  the  parallel 
composition  scheme  for  composing  all  of  the  various  desired  velocities  is  discussed.  Each  of  these  functions 
are  a  novel  stand  alone  contribution  to  the  field. 

In  Chapter  4  the  experimental  testbed  design  is  described  in  detail.  Many  of  the  experimental  components 
were  not  designed  to  operate  in  tandem.  Additionally,  several  project  specific  design  solutions  were  created 
to  achieve  the  necessary  experimental  setup. 

In  Chapter  5  the  experimentation  done  via  simulation  is  discussed.  This  chapter  includes  a  detailed 
explanation  of  the  various  metrics  used  to  determine  and  quantify  the  robustness  of  the  swarm  communication 
network.  The  results  of  the  various  experiments  are  documented  and  interpreted. 

Chapter  6  reflects  on  the  contributions  and  tasks  achieved  in  this  project.  In  addition,  ideas  for  future 
research  are  examined. 
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Figure  1.4:  A  detailed  view  of  the  steps  necessary  for  the  final  experimental  setup. 
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Chapter  2 

Background 


This  chapter  reviews  some  robot  motion  control  techniques  and  examines  the  literature  for  related  work.  As 
reflected  in  Figure  1.4,  robot  control  can  be  divided  into  three  levels.  A  task-level  control,  which  assigns  a 
desired  body  velocity  for  a  robot  to  complete  a  specific  objective  (e.g,  avoid  obstacles);  a  high-level  control 
layer  which  composes  various  task-level  velocity  commands  into  a  final  desired  body  velocity  which  achieves 
the  mission  objectives;  and  a  low-level  controller  which  implements  the  desired  velocity  produced  by  the 
high-level  controller,  by  selecting  individual  motor  commands. 


1  Task-level  control 

Many  task  level  control  techniques  are  available  (see  [20]  for  examples).  However,  only  certain  approaches 
are  appropriate  for  swarm  applications.  A  suitable  task-controller  should  be: 

•  Closed  loop:  It  must  continually  update  its  motion  plan  based  on  its  current  position,  allowing  it  to 
correct  for  localization  errors  or  noise  that  may  be  introduced  in  the  course  of  the  experiment. 

•  Reactive:  It  must  be  able  to  adapt  to  a  changing  environment.  Each  member  of  the  swarm  functions 
as  a  moving  obstacle,  leading  to  a  constantly  changing  environment. 

•  Distributed:  Each  member  of  the  swarm  should  be  able  to  synthesize  its  own  motion  plan  using  only 
locally  available  information,  without  relying  on  a  “master”  robot. 

•  Computationally  efficient:  It  must  be  able  to  recompute  its  motion  plan  in  realtime,  as  new  infor¬ 
mation  becomes  available.  Note  that  the  robot’s  on-board  processing  power  (clockspeed  of  266MHz) 
is  significantly  less  than  that  of  a  desktop  computer  (typically  1  GHz).  The  speed  at  which  motions 
must  be  computed  by  the  robot’s  on-board  computer  is  dictated  by  the  camera’s  frame  rate,  typically 
greater  than  5  Hz. 

Artificial  potential  fields  meet  all  of  these  criteria 

1.1  Artificial  potential  fields 

A  standard  and  highly  adaptable  approach  is  the  artificial  potential  field  method  [19].  The  artificial  potential 
field  method  involves  projecting  an  artificial  potential  (i.e.  scalar)  field  onto  the  domain  in  which  the  robot 
is  working.  If  x  and  y  are  coordinates  describing  the  robot’s  position  the  potential  function  is 
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It  is  possible  to  define  a  potential  field  with  the  lowest  potential  value  at  a  specific  desirable  location  or 
locations  and  highest  value  at  undesirable  locations. 

The  negative  gradient,  defined  by: 

d(j> 

x  =  ~dx 
d  4> 

V  dy 

determines  the  desired  body  velocity.  By  moving  in  the  direction  of  the  negative  gradient  the  robot  will 
move  to  a  lower  potential  in  the  shortest  amount  of  time. 

The  distance  to  a  goal  location,  the  distance  between  robots,  or  the  distance  to  the  nearest  obstacle  can 
be  encoded  in  the  function  itself.  Figure  2.1  (b)  shows  a  potential  field  for  a  goal  located  in  the  upper  right 
hand  corner.  Part  (c)  of  the  figure  shows  the  potential  function  for  the  robot  to  avoid  collision  with  the 
obstacles.  Part  (d)  of  the  figure  shows  the  superposition  of  these  two  simple  artificial  potentials.  These 
pictures  show  each  potential  field  with  the  value  of  the  field  plotted  as  a  height.  This  is  analogous  to  the 
gravitational  potential  represented  on  a  topographic  map. 


Figure  2.1:  (a)  Represents  an  overhead  view  of  the  robot’s  workspace  with  two  obstacles;  (b)  represents  the 
attractive  potential  made  by  the  goal;  (c)  shows  the  repulsive  potential  produced  by  the  obstacles;  (d)  is  the 
superposition  of  (b)  and  (c).  All  xy  axis  have  units  of  cm  and  the  z  axis  has  units  of  cm2 /s. 

For  many  potential  fields,  the  negative  gradient  can  be  calculated  symbolically.  However  many  other 
functions  representing  potential  fields  are  too  complex  or  have  a  discontinuity  so  the  negative  gradient  cannot 
be  evaluated  symbolically.  For  such  potential  fields  the  negative  gradient  must  be  found  using  numerical 
methods  according  to  the  definition  of  the  derivative 


d(j>  ^  +  A)  —  (j){ x  —  A)) 

dx  2A 
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Through  experimentation,  A  =  .OIcto  was  observed  to  provide  an  approximation  sufficiently  close  for  the 
engineering  purposes  of  this  project. 

1.2  Basic  task  potentials 

Several  schemes  exist  using  artificial  potential  fields  methods  for  the  basic  tasks  of  obstacle  avoidance  and 
goal  completion.  The  purpose  of  this  project  is  not  to  investigate  these  ideas  or  functions  any  further.  The 
main  point  of  this  project  is  to  develop  controllers  for  communication  (i.e.  maintaining  inter-robot  range 
and  line  of  sight)  and  then  combine  them  with  pre-existing  obstacle  avoidance  and  goal  completion  potentials 
in  a  novel  way.  Therefore,  standard  functions  as  outlined  in  the  literature  are  used  for  this  project.  While 
these  are  standard  techniques,  it  is  a  necessary  step  to  creating  a  fully  functional  system. 

Goal  completion  potential  functions  The  goal  completion  potential  function  is  a  shallow  paraboloid. 
A  paraboloid  is  a  parabola  rotated  about  the  z  axis  to  create  a  3  dimensional  shape,  see  Figure  2.2.  A 
paraboloid  is  mathematically  defined  as: 


0goai{x,  y)  =  {x-  a)2  +  (y-  b)2 

where  (a,  b)  is  the  Cartesian  coordinate  of  the  goal  location.  As  the  robot  closes  the  distance  to  the  goal,  the 
velocity  decreases  until  at  the  goal  the  velocity  equals  zero.  A  paraboloid  is  a  stabilizing  function.  As  the 
robot  approaches  the  goal,  its  speed  decreases  and  thus  the  robot  will  not  overshoot  the  goal  but  in  theory, 
come  to  rest  at  the  goal.  [20,  page  299] 


Figure  2.2:  A  paraboloid  with  the  goal  at  the  minimum.  Units  of  xy  axis  are  cm  while  z  axis  is  cm2 / s 


Obstacle  avoidance  potential  functions  Standard  obstacle  avoidance  functions  as  defined  by  Latombe 
are  also  used  [20,  page  300] 

<l>obst(d(x,y))  =  I N  te  “  ife)  d(x’y)  -  D°  , 

I  0  d(x,y)  >  D0 


where  d(x,  y )  is  the  distance  from  the  robot  to  the  nearest  edge  of  the  obstacle,  Dq  is  an  arbitrary  cutoff 
distance  where  the  obstacle  avoidance  function  stops  exerting  influence,  and  A  is  a  positive  scaling  factor. 
This  function  acts  as  a  “repulsive”  potential  (see  Figure  2.3).  By  changing  the  scaling  factor  and  the 
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(a) 


(b) 


Figure  2.3:  (a)  A  radial  cross  section  of  the  obstacle  avoidance  potential  function  and  (b)  the  speed  assigned 
by  this  function.  N  =  50,  D0  =  17  and  the  maximum  height  of  the  potential  =  1.  x  axis  is  in  units  of  cm 
while  the  z  axis  is  in  cm2  / s 


cutoff  distance,  the  influence  of  an  obstacle  can  be  changed.  The  direction  of  the  negative  gradient  of  </>0bst 
(and  hence  velocity)  is  perpendicular  to  the  nearest  edge  of  an  obstacle.  As  the  distance  increases  from 
the  obstacle  the  negative  gradient  tends  toward  zero,  until  the  cutoff  distance,  when  —V(j)0bst  =  0.  The 
function’s  negative  gradient  approaches  infinity  as  d(x,  y)  decreases,  which  causes  a  stronger  “repulsion”  the 
closer  the  robot  is  to  the  obstacle,  preventing  a  collision. 

While  these  potential  functions  are  standard,  both  of  these  functions  were  required  for  basic  operation 
of  a  simulation  and  implementation.  MATLAB  computer  code  can  be  found  in  Appendix  A. 

1.3  Related  work  to  communication  task  potentials 

Little  has  been  done  with  specific  regard  to  potential  functions  and  the  communications  constraints  outlined 
above.  Several  papers  have  investigated  components  of  communication  task  potentials,  but  none  has  provided 
a  specific  potential  uniquely  addressing  the  design  criteria  required  for  robust  communication. 

A  range  constraint  for  a  swarm  of  robots  was  considered  by  Reif  and  Wang  [31]  and  has  been  a  component 
of  other  research  such  as  Li  and  Rus  [21].  Reif  and  Wang’s  work  contained  a  potential  function  which  had 
several  properties  in  common  with  the  Marr  Wavelet  and  helped  serve  as  a  basis  for  the  research  regarding 
the  range  constraint.  However,  as  the  authors  point  out,  their  application  of  the  potential  function  did  not 
scale  linearly  with  the  number  of  members  in  a  swarm  and  therefore  was  not  appropriate  for  this  project. 
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In  addition,  it  does  not  allow  independent  specification  of  minimum  and  maximum  separation  distances. 

Maintaining  line  of  sight  has  been  a  component  of  other  research  ([36,  42,  3,  1]).  Yet  all  of  the  approaches 
to  maintaining  line  of  sight  rely  on  a  leader  and  follower  robot  or  only  one  robot  moving  at  a  time.  In  contrast, 
this  project  is  specifically  designed  to  be  completely  decentralized  with  no  leaders  or  followers  and  each  robot 
continuously  moving.  For  these  reasons,  new  controllers  based  upon  a  potential  field  method  of  control  had 
to  be  designed  from  first  principles  for  this  project. 

There  is  a  wealth  of  scholarship  on  redundancy  of  a  network  in  a  computer  science  context  ([14,  9,  23, 
13]).  In  addition,  the  problem  of  connectedness  ([41,  page  161])  has  been  extensively  studied  using  the 
mathematical  held  of  graph  theory  and  discrete  mathematics.  However  little  literature  exists  on  the  spatial 
arrangement  of  mobile  nodes  to  create  and  maintain  redundancy  in  a  wireless  network.  Grabowski  et  al.  [16] 
explicitly  consider  the  creation  of  a  redundant  line  of  sight  controller  with  several  robots.  However,  the 
controller  is  based  on  only  one  mobile  robot  and  several  stationary  ones  and  therefore  not  applicable  to  this 
project. 

1.4  Discussion 

The  potential  held  method  of  control  for  task  level  objectives  has  several  advantages  over  other  methods  of 
control.  Potential  helds  are  easy  to  visualize  and  there  is  a  wealth  of  physical  examples  from  which  to  draw 
inspiration  and  which  can  serve  as  intuitive  models.  Other  methods  of  control,  such  as  using  a  compensator, 
do  not  have  physical  analogies  and  cannot  be  easily  visualized.  To  design  a  potential  held,  three  dimensional 
parametric  equations  are  written  which  dehne  a  potential  hypothetically  characterizing  the  desired  stimulus 
response.  To  assess  the  potential,  simulations  of  a  robot  moving  according  to  the  negative  gradient  of  this 
potential  held  are  run  to  determine  if  the  actual  motion  meets  the  design  specifications.  This  process  is 
repeated  iteratively  until  the  desired  motion  is  achieved. 

Potential  helds  also  have  some  drawbacks,  which  if  not  properly  accounted  for  can  have  undesirable 
consequences.  Most  of  these  problems  arise  from  the  summation  or  superposition  of  multiple  potentials. 
One  problem  is  that  potential  functions  can  have  different  magnitudes  or  scales.  For  potential  helds  with 
different  magnitudes,  the  larger  magnitude  potential  held  can  overpower  the  smaller.  A  hrst  step  to  engineer 
around  this  drawback  is  normalization.  Normalizing  the  functions  is  the  process  of  scaling  the  functions  to 
a  common,  arbitrary  magnitude  so  all  the  potentials  will  have  comparable  effects  in  composition.  However, 
some  functions  or  their  negative  gradients,  for  instance  4>g0ai  are  not  bounded  and  therefore  cannot  be 
normalized.  Since  these  functions  are  unbounded  the  velocity  must  be  capped  at  a  maximum  value.  See 
Figure  2.4  for  an  example  using  (f>g0ai',  Figure  2.3  also  exhibits  the  property  of  being  capped.  The  reason  for 
setting  a  maximum  velocity  is  two-fold.  First,  this  ensures  that  a  function  does  not  inadvertently  overpower 
another  function.  Secondly,  it  is  based  on  the  physical  limitations  of  the  robot  motors.  The  motors  saturate 
at  a  certain  velocity  and  therefore,  it  is  not  an  accurate  model  of  the  physical  systems  if  the  functions 
can  return  a  near  infinite  velocity.  In  addition,  normalization  eases  the  visualization  of  the  composition  of 
functions,  as  shown  in  Figure  2.1  (d). 

A  second  difficulty  with  the  summation  of  potential  functions  is  that  the  summation  of  two  potentials 
can  produce  undesired  local  minima.  At  a  local  minimum,  the  negative  gradient  is  zero  and  thus  the  velocity 
is  zero.  These  local  minima  prevent  the  robot  from  ever  reaching  its  objective  location.  Local  minima  are 
always  a  problem  with  motion  defined  by  potential  functions,  but  it  is  especially  difficult  with  a  summation 
of  potentials  to  not  inadvertently  create  local  minima.  Even  with  carefully  designed  potential  fields,  in  the 
summation  of  a  large  number  of  potential  helds  local  minima  can  unexpectedly  occur. 

When  dealing  with  a  single  robot,  local  minima  are  a  critical  design  consideration.  However,  in  swarm 
robotics,  local  minima  are  not  as  serious.  This  project  relies  on  several  phenomena  in  order  to  engineer 
around  local  minima.  The  hrst  is  a  non-static  potential  held.  In  principle,  for  a  static  potential  held,  the 
potential  function  can  be  examined  for  these  local  minima.  A  non-static  potential  held  continuously  changes, 
for  example,  as  the  distances  between  robots  change.  Therefore,  if  a  local  minimum  does  occur,  as  the  other 
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(b) 


Figure  2.4:  (a)  A  parabola  that  changes  to  a  line  at  distance  =  3  (b)  the  speed  capped  at  a  maximum  speed 
of  4. 


robots  change  position,  the  local  minimum  could  subsequently  disappear.  Non-static  potential  fields  have 
the  disadvantage  that  they  cannot  be  inspected  and  visualized  as  easily  as  static  potential  fields.  Instead, 
simulations  must  be  run  in  order  to  determine  when  and  if  a  local  minimum  occurs.  The  simulations  also 
indicate  the  duration  of  the  local  minimum  and  how  seriously  it  affects  the  performance  of  the  swarm. 

The  second  phenomena  which  engineers  around  local  minima  is  that  swarms  are  designed  to  be  fault 
tolerant  and  can  lose  a  few  of  their  members,  e.g.  to  immobility,  yet  still  accomplish  the  overall  mission. 
This  project  intends  to  specifically  design  controllers  which  are  able  to  function  should  the  swarm  lose  a  few 
of  its  members  (Criteria  4 '.Fault  Tolerance). 

Finally,  as  will  be  described  in  Section  2.2,  potential  functions  are  not  summed  in  a  straightforward 
manner,  reducing  the  number  of  spurious  minima. 


2  High-level  control 

After  the  desired  body  velocities  are  computed  from  each  of  the  task  potentials,  a  method  needs  to  be  used 
to  compose  the  desired  body  velocities  into  a  final  velocity.  A  simple  summation  of  velocities  can  result 
in  undesired  motions  as  explained  above.  Instead,  a  high  level  control  architecture  is  used  to  combine  the 
various  body  velocities  into  a  final  velocity. 
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2.1  Work  related  to  high-level  control 

There  are  several  control  architectures  used  to  compose  task  level  velocity  commands  into  a  single  final 
velocity  command  for  the  robot.  A  subsumption  architecture  [7]  is  based  on  a  rigid  prioritization  of  the 
individual  task  level  controllers.  This  architecture  creates  algorithms  which  block  all  task-level  commands 
from  lower  priorities  if  a  higher  priority  is  activated.  This  method  is  not  well  suited  for  this  project  because 
it  forces  a  prioritization  which  will  not  allow  the  accomplishment  of  two  disparate  goals  at  the  same  time. 

Another  architecture  is  a  behavior  based  model  [2].  When  more  than  one  task-level  command  is  given  the 
velocity  commands  are  superposed  or  added.  This  technique  is  designed  to  mimic  actual  biological  systems. 
However,  the  straight  forward  addition  of  two  or  more  task-level  velocities  does  not  ensure  that  either  task 
is  accomplished,  and  may  create  local  minima. 

Another  control  architecture  is  the  redundant  control  method  which  was  developed  by  Professor  Bishop 
[6].  This  architecture  is  based  upon  techniques  used  to  control  robot  arms,  and  uses  the  extra  degrees  of 
freedom  in  the  swarm  for  controlling  swarm  wide  statistics  such  as  the  positional  mean  and  variance  of  the 
swarm  distribution.  While  this  algorithm  is  designed  specifically  for  swarms  it  is  only  used  to  achieve  swarm¬ 
wide  objectives.  It  cannot  control  the  individual  positions  of  the  robots.  Thus,  this  controller  does  not  have 
any  mechanism  in  which  to  ensure  that  the  individual  robots  are  positioned  in  such  a  way  to  guarantee  line 
of  sight  or  redundancy  and  therefore  is  not  suited  for  this  project. 

Midshipman  Tan  [37]  investigated  and  created  a  hybrid  controller  for  a  swarm  of  robots  primarily  based 
upon  the  redundant  method  with  elements  of  the  other  two  architectures.  However,  it  too  was  only  applicable 
to  swarm  wide  objectives. 

2.2  Planned  approach 

The  planned  high-level  control  approach  is  loosely  based  on  work  by  Professor  Esposito  [11].  The  basic  idea 
is  that  the  negative  gradient  direction  is  not  the  only  motion  direction  that  will  decrease  the  potential.  Any 
direction  that  consistently  decreases  the  task  potential  can  lead  to  accomplishing  the  objective.  Therefore 
there  is  some  degree  of  latitude  in  selecting  the  final  velocity. 

Essentially,  the  set  of  all  velocity  directions  should  be  constructed  for  each  task  objective.  One  must  then 
compute  the  intersection  of  the  sets.  If  the  intersection  exists,  any  velocity  in  the  intersection  can  achieve 
all  the  task-level  goals  simultaneously.  If  the  intersection  is  empty,  it  is  truly  not  feasible  to  accomplish  all 
of  these  objectives  at  once  and  some  must  be  discarded. 

The  method  is  outlined  in  [11]  as  an  abstract  proof.  The  implementation  of  this  method  is  discussed 
in  detail  in  Chapter  3  Section  4.  Additionally,  an  algorithm  was  created  for  deciding  which  desired  body 
velocities  must  be  discarded  in  order  to  achieve  a  set  of  velocities  for  which  an  intersection  exists. 


3  Low-level  control 

The  high-level  control  algorithm  returns  a  body  velocity  x  and  y  which  must  be  changed  into  motor  speeds 
so  the  robot  can  be  commanded. 

A  differential  drive  controls  the  Koala  robot’s  course  and  speed.  A  differential  drive  consists  of  motors 
attached  to  each  set  of  wheels  which  can  be  controlled  independently.  Figure  2.5  details  some  critical 
parameters  in  the  kinematic  derivation.  Let  R  be  the  wheel  radii,  W  is  the  separation  distance  between  the 
left  and  right  wheels,  and  (f>L  are  the  right  and  left  wheel  angles  respectively.  Also,  let  x  and  y  be  the 
coordinates  of  the  point  midway  between  the  left  and  right  wheels  and  6  be  the  heading  angle  of  the  robot 
as  measured  counter  clockwise  from  the  positive  x-axis. 

A  difference  in  speed  between  the  two  motors  results  in  the  robot  turning.  Only  the  motor  speeds, 
4>r,4>l,  can  be  controlled  so  a  transform  from  the  desired  course  and  speed  to  the  motor  speeds  must  be 
computed.  Since  the  wheels  cannot  create  a  velocity  perpendicular  to  their  heading,  instantaneous  motion 
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angle  of 
left  wheel 


in  this  direction  cannot  be  achieved.  Because  of  this  velocity  constraint,  the  robots  are  non-holonomic.  The 
computer  simulations  used  in  this  project  constrain  maximum  speed  but  simulate  a  holonomic  robot  which 
can  move  in  any  direction. 

A  special  matrix,  called  a  Jacobian,  transforms  the  motor  speeds  into  rates  of  change  for  xy  coordinates 
and  heading,  9.  Shown  below  is  the  Jacobian  equation  (see  Figure  2.5  for  definition  of  parameters  and 
variables): 


X 

y 

=  J 

&R 

9 

.<£l_ 

where  J  is  the  Jacobian  matrix  and  is  defined  as  follows: 

§  cos (0)  §  cos (9) 

%  sin(0)  ^  sin(0) 

_R  __R 

w  w  . 
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However,  there  are  more  outputs  than  inputs  using  this  Jacobian  matrix.  This  means  that  algebraically, 
J  is  not  invertible  and  so  one  cannot  solve  4>r,4>l  for  an  arbitrary  x,y  and  6.  To  compensate  for  this,  a 
control  point  must  be  selected  which  is  located  a  distance  D  away  from  the  axle.  By  controlling  the  x,y  of 
this  point,  a  new  Jacobian,  Jp  is  used  to  write  a  new  equation  [29]. 


cos(0)  — 
sin(0)  + 


#cos(0) 


§  cos(0)  +  4fr  sin(0) 
I  sin(0)  -  ^  cos(0) 


Note  that  D  must  be  non-zero  so  the  matrix’s  columns  are  linearly  independent.  By  taking  the  inverse 
of  this  Jacobian,  wheel  velocities  can  be  calculated  from  the  rates  of  change  of  the  xy  coordinates; 


<t>R 

_4>l_ 
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Chapter  3 


Motion  Control  Algorithm 


1  Inter-robot  separation  distance 

There  are  two  design  specifications  for  a  ranging  potential  function  for  this  project.  A  wireless  signal’s 
maximum  distance  for  effective  communication,  dc,  is  dictated  by  the  power  of  the  transmitter.  Therefore, 
the  robots  should  remain  with  inter-robot  distances  less  than  dc.  The  second  specification  is  that  the  robots 
must  also  maintain  a  minimum  separation  distance,  ds,  between  each  other  to  prevent  inter-robot  collision. 
The  potential  function  must  meet  both  design  specifications 

One  candidate  potential  function  which  appears  to  meet  both  specification  is  the  Marr  Wavelet.  This 
function  is  defined  by: 

j.  (m  w  C  n,  d2(x’V)\  f~d2{x,y)\ 

</>Marr(d(a:,y))  =  —  (h - — — )  exp  I  — — -  1 

where  d(x,  y)  is  the  distance  between  the  two  robots,  and  C ,  h  and  <r  are  parameters  defining  the  maximum, 
minimum  and  the  dispersion  of  the  function.  Originally,  this  wavelet  was  introduced  to  help  explain  biological 
vision  processing  [24].  Though  the  wavelet  was  never  intended  as  a  potential  function  for  robot  motion 
control,  it  has  many  features  which  make  it  well  suited  for  this  application. 

The  function  is  set  so  that  its  maximum  value  is  centered  on  the  location  of  a  target  robot.  When  another 
robot  evaluates  this  potential,  the  large  value  of  the  negative  gradient  near  the  center  of  the  function  at  the 
target  should  inhibit  the  two  robots  from  colliding.  Farther  from  the  location  of  the  target  robot,  the  negative 
gradient  of  the  function  points  toward  the  location  of  the  target  robot  and  thus  has  the  effect  of  pulling  the 
robots  together  for  communication.  Under  the  influence  of  this  potential,  the  robot  will  settle  at  the  location 
of  the  minimum  value  of  the  potential.  The  location  of  the  minimum  value  for  the  function  represents  the 
desired  distance  of  separation  for  avoiding  collision  and  effective  communication. 

Simulation  makes  obvious  that  the  Marr  Wavelet  is  not  effective  at  bringing  other  robots  into  communica¬ 
tion  range.  At  large  values  of  d(x,  y),  the  negative  gradient  of  the  Marr  Wavelet  approaches  zero.  Therefore, 
the  evaluating  robot  will  have  almost  no  velocity  which  would  bring  it  within  communication  range  of  the 
target  robot. 

A  second  function  which  could  correct  the  deficiencies  of  the  Marr  Wavelet  is  a  shallow  parabola,  defined 
by: 

<l>Sp(d(x,y))  =  kd2(x,y) 

where  k  <C  1  and  d(x,y)  is  the  distance  between  the  robots. 

The  magnitude  of  the  negative  gradient  of  a  shallow  parabola  increases  as  the  distance  between  the  robots 
increases.  Thus  the  shallow  parabola  potential  function  can  be  used  to  maintain  range  for  communication 
even  when  the  robots  are  at  great  distances  apart.  At  small  values  of  d(x,  y)  the  negative  gradient  approaches 
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Figure  3.1:  Function  </>range  with  ds  =  1  dm  and  dc  =  3dm.  y  axis  has  units  of  cm2 / s 


zero  and  thus,  this  function  does  not  work  well  for  maintaining  the  separation  distance  ds .  The  Marr  Wavelet 
function  works  well  for  avoiding  inter-robot  collision,  but  fails  at  long  distance,  whereas  a  shallow  parabola 
potential  works  well  at  long  distances  and  has  negligible  effect  at  close  distances. 

In  order  to  capitalize  on  the  advantages  that  each  of  these  functions  present,  a  new  function,  <j>Ta nge, 
composed  of  the  best  of  both  is  created.  In  addition,  as  long  as  the  robot  is  between  the  two  values  of  ds 
and  dc,  the  ranging  potential  function  should  have  — V0range  =  0.  Thus:  (Figure  3.1) 

!<t>  M  arr  (d(x,y))  d(x,y)<ds 
0  ds  ^  d(x^  y)  ft  dc  • 

0sp  (d(x,y))  dc<d(x,y ) 

MATLAB  computer  code  can  be  found  in  Appendix  B.  The  advantages  of  this  function  include: 

•  successfully  meets  the  two  design  specifications  of  maintaining  a  minimum  separation  distance  and  a 
maximum  communication  distance  i.e.  stable  minimum  when  ds  <  d  <  dc ; 

•  separation  distance  and  maximum  communication  distance  can  be  set  independently, 

•  easy  to  change  values  based  on  differing  conditions  or  requirements; 

•  the  function  is  smooth  so  that  a  derivative  exists  everywhere;  and 

•  no  extraneous  velocity  returned  when  robot  is  within  specifications. 

2  Line  of  sight 

The  design  criteria  for  line  of  sight  control  is  based  upon  the  following  scenario:  as  Robot  A  loses  line  of 
sight  with  Robot  B  (Fig.  3.2),  Robot  A  should  have  a  velocity  which  maintains  or  re-establishes  line  of 
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sight.  Conceptually  then,  for  Robot  A  to  maintain  line  of  sight,  the  potential  function  should  return  a  gentle 
velocity  out  of  the  occlusion  zone  of  the  obstacle  with  respect  to  Robot  B.  It  is  important  to  note  that  in  the 
full  scenario,  Robot  B  will  also  be  moving  and  evaluating  an  identical  control  law  which  is  governing  Robot 
A. 


Figure  3.2:  The  example  scenario  for  the  line  of  sight  problem  with  a  stationary  robot  and  an  obstacle. 

A  solution  to  the  above  scenario  is  encoded  in  the  potential  function  </>LOs(Fig.  3.3).  An  upper  and 
lower  occlusion  angle  ( 6 )  and  a  radius  from  the  stationary  robot  describe  the  occlusion  zone  mathematically 
with  polar  coordinates.  The  only  information  Robot  A  needs  to  generate  the  potential  function  is  Robot 
B’s  position  and  the  shape  of  the  obstacle.  For  this  function,  it  is  more  instructive  to  define  the  negative 
gradient  directly.  For  </>los>  the  negative  gradient  will  always  point  perpendicular  to  the  nearest  occlusion 
line,  so  that  the  robot  exits  the  occlusion  zone  in  the  quickest  manner  possible.  If  d{x1  y)  is  the  Euclidian 
distance  to  the  nearest  occlusion  line  (which  has  an  angle  of  6)  and  s  is  a  small  positive  number,  then  for 
d(x,  y)  +  s  >  0, 

cos(tan(0))(d(x,  y)  +  s ) 
sin(tan(0))(d(x,  y)  +  s) 

If  d(x,y )  +  s  <  0  then  —  V</>los  =  0-  The  offset  s  is  included  so  that  a  small  velocity  is  present  outside  of 
the  occlusion  zone  to  help  maintain  line  of  sight.  <(>los  is  the  best  choice  for  maintaining  line  of  sight  for 
these  reasons: 

•  velocity  is  perpendicular  to  occlusion  lines  so  line  of  sight  is  re-established  in  shortest  amount  of  time 
possible; 

•  a  proportionate  velocity  to  distance  out  of  communication  range;  and 

•  an  ‘offset’  to  help  maintain  line  of  sight. 


-V<^los  = 


26 


Figure  3.3:  A  simulation  of  a  robot  moving  under  ^los-  The  velocity  field  created  by  —  V</>los  is  marked  in 
blue.  The  initial  position  of  the  robot  is  marked  with  an  o  and  the  final  position  is  marked  with  an  x.  The 
red  lines  mark  the  occlusion  lines,  xy  axes  are  in  units  of  decimeters 


MATLAB  computer  code  can  be  found  in  Appendix  C. 

Simulations  revealed  that  the  function  works  extremely  well  for  only  one  robot  evaluating  the  potential 
function.  It  does  not  work  well  when  both  robots  are  evaluating  and  moving  according  to  the  potential 
function.  In  some  scenarios,  the  robots  will  continuously  rotate  around  an  obstacle  as  each  is  moving  to  a 
different  side  to  try  to  re-establish  line  of  sight.  In  other  cases,  if  the  obstacle  is  positioned  between  the 
robots  and  a  goal,  both  robots  will  come  to  rest  in  a  local  minimum  created  by  the  goal  function  and  the 
line  of  sight  potential  function.  This  local  minimum  means  that  the  robots  cannot  complete  their  goal  if  line 
of  sight  is  to  be  maintained. 

The  solution  to  this  problem  is  to  emulate  in  the  original  scenario  (Figure  3.2).  When  Robot  A  evaluates 
'/’los  a  body  velocity  is  returned.  In  addition,  when  Robot  B  evaluates  the  potential  function  (since  it  is 
in  the  occlusion  zone  of  A),  a  velocity  will  also  be  returned.  However,  Robot  B’s  returned  velocity  can  be 
suppressed  so  that  the  velocity  equals  zero.  This  scheme  of  suppressing  one  robot’s  returned  velocity  requires 
that  both  Robots  A  and  B  are  be  in  agreement  as  to  which  one  of  them  will  suppress  the  velocity  returned 
by  </>los-  Suppressing  a  velocity  does  not  mean  that  Robot  B  will  be  stationary,  but  rather  that  the  desired 
velocity  to  maintain  line  of  sight  with  Robot  A  will  equal  zero.  All  other  desired  body  velocities  from  the 
other  potential  functions  will  still  exert  an  influence  on  Robot  B. 

To  determine  which  robot  suppresses  its  returned  velocity  the  number  of  communication  links  for  each 
robot  is  evaluated.  The  robot  which  has  a  greater  number  of  communication  links  suppresses  its  returned 
velocity.  Moving  around  the  obstacle  to  establish  line  of  sight  with  the  other  robot  might  break  other 
communication  links  and  since  the  overall  goal  is  to  maximize  the  communication  between  robots,  the 
greater  number  of  communication  links  should  be  preserved.  Therefore,  the  robot  with  the  greater  number  of 
communication  links  will  continue  on  its  original  course  to  preserve  its  links  while  the  other  robots  maneuver 
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to  maintain  line  of  sight.  If  the  number  of  communication  links  is  equal,  then  an  arbitrary  measure,  such  as 
the  serial  number  is  used  to  determine  which  robot  suppresses  the  returned  velocity. 

It  is  important  to  note  that  this  scheme  does  require  a  minimum  of  message  passing  between  the  robots. 
Each  robot  needs  to  pass  to  the  other  the  number  of  communication  links  it  has  and  possibly  its  serial 
number.  Using  a  position  dependant  characteristic  (such  as  which  robot  is  closer  to  the  goal)  was  considered 
as  a  method  of  determining  which  robot  should  suppress  its  returned  velocity.  The  advantage  of  this 
method  is  that  it  does  not  require  message  passing,  but  only  uses  the  information  the  robot  already  has, 
namely  the  position  of  the  other  robot.  However,  this  type  of  scheme  does  not  consider  the  number  of 
communication  links  per  robot  and  thus  does  not  maximize  a  robust  communication  network  (Criteria 
2: Robust  Communication). 

This  message  passing  does  not  violate  design  Criteria  3: Distributed  Operation  which  states  that  “each 
robot  makes  its  own  motion  control  decisions  based  on  locally  available  information.”  All  information  can 
be  obtained  locally  by  communicating  with  the  other  robot  as  the  two  robots  approach  a  situation  which 
could  interrupt  line  of  sight  communication. 

When  this  control  law  is  combined  with  other  task  potentials  using  the  communication  variant  of  the  par¬ 
allel  composition  controller,  line  of  sight  will  always  maintained.  Simulations  confirm  this  result  (Figure  3.4). 
MATLAB  computer  code  for  this  parallel  composition  controller  can  be  found  in  Appendix  E  and  L. 
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Figure  3.4:  A  simulation  of  a  two  robots  moving  according  to  the  communication  variant  control  law  (Sec¬ 
tion  4)  so  that  they  always  maintain  line  of  sight  as  they  move  towards  the  goal,  xy  axes  have  units  of 


cm. 
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3  Redundancy 

At  a  minimum,  a  communication  network  is  redundant  if  each  robot  maintains  at  least  two  communication 
links.  If  this  condition  is  met,  a  single  robot  could  fail,  and  yet  all  robots  would  still  be  in  communication 
with  at  least  one  other  robot.  By  requiring  more  than  two  connections  per  robot,  a  stronger  redundancy 
can  be  created.  Later  in  Chapter  5  the  amount  and  degree  of  redundancy  in  a  network  is  examined  in  more 
detail. 

A  first  attempt  at  achieving  redundancy  was  to  create  a  potential  function  which  would  orient  the 
network  nodes  in  such  a  way  as  to  create  redundancy.  After  careful  study  and  simulation,  this  approach 
was  abandoned  as  it  proved  to  be  untenable  and  a  more  efficient  method  of  creating  redundancy  was  found. 
However,  the  efforts  of  this  first  attempt  warrant  some  discussion.  It  was  discovered  that  the  type  of  functions 
explored  cannot  be  scaled  linearly  and  therefore  are  not  good  potential  functions  for  swarm  robotics. 

3.1  Potential  Functions  for  Redundancy 

A  first  attempt  at  achieving  redundancy  was  to  have  three  robots  moving  toward  a  common  point.  The 
hypothesis  is  that  the  robot  motions  are  coordinated  in  moving  towards  a  common  point,  and  thus  would 
establish  redundant  communication  expeditiously.  To  avoid  inter-robot  collisions  as  they  approach  the 
common  point,  a  ranging  potential  function  such  as  the  Marr  Wavelet  would  be  centered  on  the  common 
point  to  keep  the  robots  at  a  safe  distance  from  each  other. 

The  class  of  geometrical  constructions  known  as  the  centers  of  a  triangle  serve  as  a  natural  starting 
place  for  finding  a  common  point  for  three  robots.  The  locations  of  the  robots  would  serve  as  the  vertices 
of  the  triangle  and  a  center  of  this  triangle  would  be  the  common  destination  point  for  the  robots.  By 
carefully  selecting  which  of  the  many  possible  centers  serves  as  the  destination  point,  other  advantages  are 
attained.  While  the  main  focus  is  still  on  establishing  communication,  the  properties  of  a  center  which  could 
be  advantageous  to  the  swarm  determined  which  of  the  many  possible  centers  were  used. 

The  circumcenter  is  a  center  of  a  triangle  which  is  also  the  center  of  a  circle  passing  through  the  three 
vertices.  Therefore,  this  center  is  equidistant  from  all  vertexes.  Because  the  distances  are  equal,  the  robots 
will  arrive  at  the  circumcenter  simultaneously,  no  robot  waits  for  the  others  and  so  it  seems  redundant  com¬ 
munication  should  be  established  as  quickly  as  possible.  However,  simulations  with  this  function  demonstrate 
that  this  is  not  a  viable  option  for  quickly  establishing  redundant  communication.  For  an  obtuse  triangle 
the  circumcenter  lies  outside  of  the  triangle.  For  very  obtuse  triangles,  the  circumcenter  is  located  a  large 
distance  from  the  vertices  (Fig.  3.5).  Thus,  for  an  obtuse  triangle,  the  common  point  of  the  circumcenter  is 
a  poor  choice  for  quickly  establishing  redundant  communication. 

Another  center  of  a  triangle  which  has  attractive  properties  is  the  Fermat  point  which  is  also  known  as 
the  isogonic  or  Rorricelli  point.  This  center  minimizes  the  sum  of  the  distances  from  the  three  vertices. 
In  addition,  the  three  angles  between  the  lines  connecting  the  vertexes  to  the  Fermat  point  are  all  120 
degrees.  [8]  (Fig.  3.6) 

The  Fermat  point  only  minimizes  distances  on  triangles  with  all  angles  less  than  120  degrees.  For  very 
obtuse  triangles,  the  Fermat  center  no  longer  lies  inside  the  triangle,  instead  the  point  of  minimum  distance 
is  the  vertex  which  has  an  angle  of  120  degrees  or  greater. 

The  advantage  of  using  the  Fermat  point  is  that  it  can  minimize  energy  expended  by  the  swarm.  The 
farther  a  robot  travels,  the  more  energy  is  used.  Minimizing  the  sum  of  the  distances  each  robot  travels 
also  minimizes  the  total  energy  expended  by  the  swarm.  One  robot  may  expend  more  energy  moving  to  the 
Fermat  point,  but  the  total  energy  used  by  the  system  is  minimized.  In  swarm  robotics,  the  focus  is  not 
on  individual  energy  expenditure,  but  rather  on  aggregate  energy  used  by  the  swarm.  Thus,  the  minimal 
distance  point  is  of  special  interest  to  swarm  robotics. 

Simulations  demonstrated  that  using  the  Fermat  point  as  a  common  point  works  well  for  three  robots. 
However,  simulations  also  reveal  that  this  approach  cannot  guarantee  redundancy  for  swarms  larger  than 
three  members.  When  a  swarm  has  more  than  three  members,  the  robots  do  not  necessarily  group  themselves 
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Figure  3.5:  The  robots  are  located  at  the  triangle  vertices  while  the  “+”  marks  the  circumcenter.  As  can 
be  seen,  for  an  obtuse  triangle  the  circumcenter  lies  outside  of  the  triangle.  Note  the  different  scale  of  the 
images. 
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Figure  3.6:  A  triangle  detailing  how  the  Fermat  center  is  constructed.  The  main  triangle  is  plotted  with 
solid  lines  with  its  Fermat  center  marked  with  a  small  circle.  The  dashed  lines  are  used  for  constructing  the 
Fermat  Center. 

into  non-overlapping  triangles.  Thus,  not  all  three  robots  in  a  triangle  are  moving  to  the  same  common 
point,  causing  some  robots  to  not  be  in  redundant  communication.  MATLAB  computer  code  related  to  this 
simulation  can  be  found  in  Appendix  D. 

The  approach  of  using  a  common  destination  point  based  on  centers  of  a  triangle  is  appealing  because 
other  advantages  can  be  achieved  from  the  properties  of  the  center.  However,  unless  the  robots  are  forced 
into  non- overlapping  groups  of  three,  this  approach  cannot  guarantee  redundancy. 

Forcing  the  robots  into  non-overlapping  groups  of  three  is  not  a  viable  option  because  it  is  not  scalable. 
Even  if  the  number  of  members  in  a  swarm  is  a  multiple  of  three,  grouping  the  robots  does  not  scale 
linearly.  As  the  number  of  robots  in  a  swarm  increases,  the  time  required  to  group  the  robots  becomes 
unreasonable.  Grouping  the  members  intelligently,  based  upon  their  spatial  arrangement,  rather  than  on 
something  arbitrary,  like  a  serial  number,  requires  an  impractical  amount  of  time.  In  order  to  group  the 
robots  successively,  based  upon  which  three  have  the  shortest  distances  between  them,  requires 
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calculations  of  distance  where  N  is  the  number  of  robots  in  the  swarm.  For  just  12  robots  in  a  swarm,  each 
robot  must  perform  975  calculations  of  distance  each  iteration. 

A  fast  central  processor  could  perform  the  calculations  necessary  to  group  the  robots,  so  that  less  powerful 
hardware  on  board  the  robots  is  not  overtaxed.  Yet  this  violates  a  central  principle  of  this  project  which  is 
to  keep  the  control  decentralized.  Each  robot  should  be  autonomous  and  not  rely  on  calculations  performed 
by  a  central  processor. 

3.2  Method  for  achieving  redundancy 

However,  the  robots  do  not  necessarily  need  to  be  moving  to  a  common  point  to  achieve  redundancy.  Instead, 
as  long  as  each  robot  is  in  communication  with  at  least  two  others,  then  redundant  communication  has  been 
achieved.  Thus,  a  robot  should  be  constrained  in  its  motion  so  that  it  is  attempting  to  maintain  range  and 
line  of  sight  with  two  robots  -  regardless  of  how  the  other  robots  are  moving.  This  means  that  each  robot 
will  be  constrained  by  four  velocities:  a  pair  of  range  and  line  of  sight  task  velocities  for  each  of  the  two 
robots  its  trying  maintain  communication  with.  A  new  potential  function  is  not  needed,  but  rather  a  method 
for  using  these  velocities  as  a  constraint  on  the  robot’s  motion.  The  planned  high-level  control  method  of  a 
parallel  composition  controller  can  easily  be  adapted  to  incorporate  4  communication  velocity  constraints. 
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4  Parallel  Composition  Controller 

4.1  Theoretical  approach  to  composition  of  task  velocities 

The  theoretical  approach  of  the  high  level  control  is  that  the  negative  gradient  direction  is  not  the  only 
motion  direction  that  will  take  a  robot  to  the  task’s  goal.  Any  direction  that  consistently  decreases  the  task 
potential  can  lead  to  accomplishment  of  the  objective.  Therefore  there  is  some  degree  of  latitude  in  selecting 
the  final  velocity.  Each  task  velocity  constrains  the  final  velocity  (x,y)  to  be  such  that: 


&4>  d(f> 

X 

dx  ’  dy 

y_ 

<  0. 


Geometrically  this  means  that  at  any  point,  a  velocity  within  ±90°  from  the  negative  gradient  will  decrease 
the  task  potential  (Figure  3.7).  Thus  the  desired  body  velocities  are  constrained  to  a  180°  arc.  By  exploiting 
this  latitude  for  several  vectors,  it  can  be  possible  to  find  one  single  vector  that  will  consistently  decrease  all 
of  the  task  potentials  and  thus,  one  direction  will  meet  several  of  the  goals  simultaneously. 


Set  of  all  directions 
which  decrease  the  potential 


Figure  3.7:  Any  vector  within  90°  of  the  negative  gradient  will  decrease  the  potential  function. 

However,  not  all  sets  of  vectors  can  be  reduced  to  one  composite  velocity  which  decreases  all  potentials. 
Thus,  it  must  be  determined  if  a  set  of  task  vectors  are  feasible.  In  order  to  be  feasible,  all  vectors  must  lie 
on  the  same  half  plane  which  is  determined  by  satisfying  the  following  statement: 


(3Vi)l(VV,-)[(V;  x  Vj)  >  0  v  (F,  x  Vj)  <  0], 


If  no  Vi  exists,  then  the  algorithm  described  in  the  next  section  is  used  to  discard  some  of  the  velocities  until 
a  feasible  set  of  vectors  is  constructed. 

Geometric  speaking  the  above  equation  is  equivalent  to  taking  the  intersection  of  all  180°  velocity  con¬ 
straint  arcs  (Figure  3.8)  and  determining  if  the  intersection  is  non-empty.  A  non-empty  intersection  is  the 
cone  of  feasible  velocities  which  can  decrease  all  of  the  potentials.  If  the  intersection  of  the  arcs  is  empty, 
then  again,  some  of  the  task  potentials  must  be  discarded. 

The  problem  of  determining  if  a  set  of  2-D  vectors  is  feasible  simplifies  to  examining  if  there  exists  an 
exterior  angle  which  is  greater  than  180°  (Figure  3.9).  If  this  is  the  case,  then  the  intersection  will  be 
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Figure  3.8:  The  intersection  of  all  constraints  determines  the  set  of  feasible  velocity  directions. 
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Figure  3.9:  (a)  a  set  of  vectors  for  which  it  is  feasible  to  compose  into  a  final  velocity  to  decrease  all  potentials, 
(b)  a  set  of  vectors  with  no  such  feasible  direction,  as  determined  by  the  exterior  angles. 


non-empty  and  a  velocity  can  be  found  which  decreases  all  potentials.  The  two  vectors  which  make  up  this 
exterior  angle  are  the  limiting  vectors  and  are  used  to  construct  the  composite  velocity. 

Given  a  set  of  feasible  velocities  a  final  composite  velocity  is  composed  based  on  the  ideas  of  Esposito 
and  Kumar  [11].  However,  Esposito  and  Kumar  only  consider  a  direction  field  (i.e.  only  unit  vectors)  rather 
than  a  velocity  field.  Secondly,  the  authors  assign  a  rigid  prioritization  of  only  two  directions  to  compose  a 
final  velocity. 

There  can  be  any  number  of  vectors  in  the  final  set  of  feasible  vectors  but  only  two  vectors  from  this  set, 
the  limiting  vectors,  determine  which  direction  will  be  the  final  velocity.  The  inside  orthogonal  vectors  to 
the  limiting  vector  are  labeled  bias  vectors  (Figure  3.10).  These  vectors  represent  the  maximum  deviation 
from  the  limiting  vectors  which  will  still  decrease  that  task  potential.  Thus,  the  arc  between  the  bias  vectors 
defines  an  arc  of  velocity  directions  which  will  simultaneously  decrease  all  potentials.  Next,  within  this  arc, 
a  direction  and  magnitude  for  the  composite  velocity  must  be  chosen.  To  accomplish  this,  the  magnitudes 
of  the  limiting  vectors  are  used  with  the  direction  of  the  closest  bias  vector  (Figure  3.11).  The  resulting 
vectors  are  then  summed  using  vector  addition.  By  using  vector  addition,  the  magnitudes,  and  therefore, 
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the  urgency  of  the  task  velocities  are  reflected  in  both  the  magnitude  and  the  direction  of  the  final  velocity. 


Limiting 

vectors 


vector 


Figure  3.10:  The  arc  of  feasible  directions  are  determined  by  the  angle  defined  by  the  bias  vectors. 


Figure  3.11:  The  composition  of  the  final  velocity  based  on  the  magnitudes  of  the  nearest  limiting  vectors 
and  ±90°  direction  from  the  more  distant  limiting  vector. 

A  special  case  of  this  composition  scheme  is  when  the  limiting  vectors  are  less  than  90°  apart.  In  this 
cases,  both  limiting  vectors  will  lie  within  the  arc  of  feasible  directions  (Figure  3.12).  Therefore,  vector 
addition  of  the  limiting  vectors  can  be  used  to  compose  a  final  velocity. 

4.2  Algorithm  for  finding  a  feasible  set  of  vectors 

Should  the  set  of  task  potentials  not  result  in  a  feasible  set  of  vectors,  an  algorithm  is  needed  for  determining 
how  to  compose  a  subset  of  vectors  which  is  feasible.  This  algorithm  is  illustrated  in  Algorithm  1. 

At  each  iteration  a  robot  evaluates  the  potential  functions  for  the  go-to-goal  function  and  obstacle 
avoidance  which  each  return  a  suggested  velocity  (or  velocities  in  the  case  of  multiple  obstacles).  In  addition, 
a  robot  individually  evaluates  the  range  and  line  of  sight  potential  functions  with  every  other  robot.  These 
potentials  return  a  suggested  velocity  LOSi  and  Ri  for  the  ith  robot.  The  suggested  velocity  is  based  on  the 
negative  gradient  as  descried  in  Chapter  2  Section  1.  From  this  set  of  suggested  velocities  a  feasible  set  of 
vectors  must  be  found  so  a  final  velocity  can  be  composed  in  the  manner  described  above. 
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Figure  3.12:  The  composition  of  the  final  velocity  for  the  case  when  the  limiting  vectors  are  less  than  90° 
apart. 


Algorithm  1  The  Algorithm  for  generating  a  feasible  set  of  vectors 
P  <—  set  of  high  priority  vectors 

Cgum  [( LOSi  +  Rl),  ( LOS2  +  R2 ),  {LOS3  +  R3), . . .  ] 
while  P  is  infeasible  do 
P  =  P  —  min(P) 
evaluate  P  for  feasibility 

end  while 

A  =  P 

M  ‘ —  number  of  desired  communication  links 
Q  <—  number  of  actual  communication  links 
for  (M  —  Q)  do 
3  (i)[(LOSi  +  Rt)  =  min(CSUTO)] 
if  LOSi  U  A  is  feasible  then 
A  =  LOSi  U  A; 
end  if 

if  Rt  U  A  is  feasible  then 
A  =  Ri  U  A; 

end  if 

O sum  —  Osurn  -  ( LOSi  +  Ri) 

end  for 


Given  a  set  of  infeasible  vectors,  the  first  step  to  finding  a  feasible  subset  is  to  group  the  vectors  based  on  a 
loose  prioritization  scheme.  The  high  priority  signals  include:  go-to-goal,  obstacle  avoidance  and  maintaining 
the  minimum  separation  distance  between  robots.  The  lower  priority  signals  are  all  of  the  communication 
vectors. 

The  high  priority  signals  are  checked  to  see  if  by  themselves  they  can  be  composed  into  a  feasible  velocity. 
If  so,  these  vectors  are  labeled  A  -  the  set  of  vectors  which  will  eventually  be  composed  into  a  final  velocity. 
If  not,  then  the  high  priority  velocity  with  the  smallest  magnitude  is  discarded.  The  potential  functions  are 
defined  in  such  a  way  so  that  the  larger  a  velocity’s  magnitude  is,  the  higher  the  urgency.  Thus,  discarding 
the  smallest  vector  is  discarding  the  least  urgent  velocity.  At  the  next  iteration,  all  potential  functions  will  be 
evaluated  and  so  this  velocity  is  only  discarded  for  one  iteration.  This  process  of  discarding  vectors  continues 
until  a  feasible  set  of  high  priority  signals  is  found  which  is  labeled  A.  A  feasible  subset  is  guaranteed  to  be 
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found  because  at  a  worst  case,  there  will  be  two  vectors  and  for  all  sets  of  two  vectors  a  feasible  velocity  can 
be  found.  1 

Next,  the  communication  subroutine  is  used  to  find  those  communication  velocities,  LOSi  and  Ri,  such 
that  the  union  with  A  is  still  a  set  of  feasible  vectors.  The  actual  number  of  communication  links  per  robot, 
Q,  is  compared  against  M,  the  desired  number  of  communication  links  per  robot.  The  communication 
subroutine  seeks  to  find  velocities  which  will  put  the  robot  in  communication  with  M  —  Q  more  robots.  Thus 
by  following  these  velocities  eventually  M  =  Q. 

This  project  requires  that  both  range  and  line  of  sight  be  attained  before  communication  is  established. 
Thus,  both  range  and  line  of  sight  velocities  for  the  ith  robot  are  evaluated  for  feasibility  together.  The 
pair  of  communication  vectors  which  require  the  least  amount  of  energy  (i.e.  the  smallest  magnitude  of 
LOSi  +  Ri)  are  selected  first.  If  either  LOSi  or  Ri  are  feasible  with  the  current  A,  the  vector(s)  is/are 
appended  to  A  and  a  counter  is  incremented  by  1.  Successive  feasibility  tests  of  LOSi  and  Ri  with  A  are 
conducted  until  the  counter  is  equal  to  M  —  Q  or  no  more  vectors  are  left  to  be  tested. 

However,  LOSi  is  tested  for  feasibility  first.  This  induces  a  slight  bias  towards  completing  line  of  sight 
before  the  range  constraint.  While  LOSi  may  be  feasible  with  A,  Ri  may  not  be  feasible  with  the  set  of 
vectors  LOSi  U  A.  If  obstacles  are  assumed  to  block  communications  (such  as  a  dense  wall)  then  decreasing 
the  range  without  being  in  line  of  sight  will  not  help  establish  communications,  the  robots  must  first  be  line 
of  sight  before  being  in  range  will  establish  communication.  If  LOSi  is  not  feasible  then  R,  is  still  examined 
to  see  if  it  is  feasible  with  the  current  A. 

When  the  counter  equals  M  —  Q  or  no  more  communication  pairs  remain  to  be  tested,  A  is  composed 
into  a  final  velocity  in  the  manner  described  in  the  previous  section.  This  algorithm  achieves  the  goal  of 
meeting  the  dual,  and  sometimes  conflicting  requirements  of  executing  a  primary  mission  while  at  the  same 
time  establishing  a  robust  communication  network. 

4.3  Communication  Variant 

In  addition  to  the  above  algorithm,  a  communication  variant  was  tested  to  determine  its  effect  on  creating  a 
robust  network.  The  algorithm  is  exactly  the  same,  except  that  the  go-to-goal  function  is  no  longer  considered 
a  high  priority.  Instead,  there  are  three  classes  of  priorities.  Class  one  consists  of  obstacle  avoidance  and 
maintaining  minimum  separation,  essentially  the  requirements  for  safe  navigation  of  the  robots.  Class  two 
consists  of  the  communication  vectors  and  the  third  class  only  contains  the  go-to-goal  function. 

The  algorithm  proceeds  in  the  same  manor  described  above  for  the  first  two  classes.  Once  an  A  has  been 
determined,  the  go-to-goal  suggested  velocity  is  checked  for  feasibility  with  this  A.  If  feasible,  this  suggested 
velocity  is  amended  to  A  and  the  final  composite  velocity  is  calculated.  However,  if  it  is  not  feasible,  A  is 
immediately  calculated  into  a  final  velocity. 

The  motivation  for  testing  this  controller  was  to  see  the  effect  of  prioritizing  communication  over  the 
go-to-goal  function  on  the  overall  effectiveness  of  the  swarm  in  completing  its  dual  missions.  Secondly,  it  was 
motivated  to  develop  a  controller  which  could  guarantee  that  line  of  sight  could  be  achieved.  While  LOSi 
by  itself  would  establish  line  of  sight,  it  is  often  discarded  by  the  first  controller.  The  line  of  sight  potential 
is  evaluated  when  the  robots  are  around  an  obstacle.  In  these  cases  usually  the  obstacle  avoidance  function 
is  returning  a  non-zero  vector.  Thus  the  high  priority  vectors  of  the  go-to-goal  and  obstacle  avoidance  are 
activated  and  almost  always  ‘lock  out’  the  line  of  sight  vector  from  being  feasible.  By  changing  the  go-to- 
goal  function  to  be  a  lower  priority  than  the  line  of  sight  velocities,  the  go-to-goal  function  will  be  infeasible 
and  thus  discarded.  Simulations  demonstrate  that  using  this  controller  will  always  guarantee  line  of  sight. 
Simulations  verify  this  result,  (Figure  3.13).  MATLAB  computer  code  for  both  controllers  can  be  found  in 
Appendix  E. 


1  The  worst  case  is  two  vectors  exactly  180°  apart.  For  this  case,  there  are  two  vectors  (±90°  of  the  vectors)  which  neither 
decreases  nor  increases  the  potentials.  None  the  less,  in  the  worst  case  the  vectors  can  be  composed  into  a  velocity 
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Figure  3.13:  Two  simulations  of  a  two  robots  moving  according  to  either  the  original  or  the  communication 
variant  of  the  controller.  The  starting  and  ending  positions  of  the  robots  are  the  same  in  each  simulation. 
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Chapter  4 

Experimental  Setup 


Simulations  provide  much  useful  information,  and  serve  as  a  flexible  test  bed  for  examining  several  different 
scenarios.  A  simulated  environment  can  be  easily  rearranged  to  provide  several  different  configurations  of 
obstacles  and  several  different  numbers  of  robots.  However,  simulations  do  not  provide  the  experience  and 
insight  of  testing  the  controller  in  the  real  world.  A  second  phase  of  testing  and  experimentation  of  the 
controller  is  to  implement  the  controller  on  the  Koala  robots. 

The  Weapons  and  Systems  Engineering  Department  purchased  eight  Koala  robots  (see  Figure  1.2)  from 
the  Swiss  company  K-team.  For  the  most  part,  these  robots  have  not  been  used  in  any  projects,  experiments 
or  research.  No  one  in  the  department  had  any  experience  programming  or  working  with  them.  The  robots 
came  with  very  little  documentation  and  instructions.  The  operation  and  configuration  of  the  robots  had  to 
be  deduced  through  experimentation  and  testing. 

Several  distinct  challenges  had  to  be  overcome  before  the  robots  could  be  used  for  demonstrations,  testing 
and  experimentation.  The  basic  experimental  set  up  is  shown  in  Figure  4.1  with  a  detailed  block  diagram 
in  figure  Figure  4.2.  Whenever  possible,  standard  techniques  were  used  for  the  experimental  setup  so  that 
a  majority  of  the  research  time  could  be  spent  on  researching  and  evaluating  the  motion  control  algorithm. 
However,  project  specific  implementations  had  to  be  designed  and  some  non-standard  techniques  were  used. 


1  Localization  via  computer  vision 

The  first  challenge  to  creating  the  necessary  experimental  setup  was  the  problem  of  localization,  i.e.  a  robot 
knowing  its  position  in  relation  to  the  environment.  Localization  is  a  principal  challenge  in  any  mobile 
robotics  experiment.  The  purpose  of  this  project  is  to  investigate  a  novel  controller  to  accomplish  the  two 
disparate  goals  of  robust  communication  and  primary  goal  completion,  not  to  investigate  a  novel  technique 
for  the  problem  of  localization.  Providing  a  solution  to  the  problem  of  localization  was  a  necessary  step  for 
creating  an  experimental  setup  which  could  test  and  investigate  the  novel  controller. 

The  solution  designed  for  this  project  was  to  employ  a  single  ceiling  mounted  overhead  camera  to  re¬ 
produce  the  position  information  which  could  be  gained  from  GPS  receivers  or  local  sensors.  Reproducing 
position  information  in  this  manor  is  a  common  approach  and  allows  for  the  complex  problem  of  localization 
to  be  dealt  with  using  the  standard  techniques  of  computer  vision.  The  design  of  the  computer  vision  system 
required  interfacing  a  camera  with  the  MATLAB  environment  so  image  processing  could  be  facilitated.  In 
addition,  a  scheme  for  identifying  and  orienting  each  robot  had  to  be  designed. 
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Figure  4.1:  A  diagram  of  the  overall  experimental  setup. 
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Figure  4.2:  A  detailed  view  of  the  steps  necessary  for  the  final  experimental  setup. 
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1.1  Computer  vision  hardware 

The  camera  used  in  the  vision  system  is  a  FireFly2  from  Point  Grey  Research.  This  camera  was  selected 
because  it  can  provide  a  high  resolution  (640x480  pixels)  color  image  in  standard  formats  for  image  processing 
(See  Appendix  F  for  technical  specifications).  Using  the  image  acquisition  toolbox  for  the  MATLAB  high 
level  mathematical  programming  language  in  conjunction  with  the  IEEE  1394  device  driver,  a  MATLAB 
program  is  able  to  capture  an  image  which  can  be  easily  used  for  image  processing.  The  camera  is  hung 
from  the  ceiling  using  a  mounting  bracket  created  by  George  Burton  in  the  Machine  Shop,  see  Figure  4.3. 
This  bracket  allows  the  camera  to  be  hung  from  the  drop-ceiling  struts  in  a  secure  manner  so  that  the  lens  is 
parallel  to  the  floor.  This  maximizes  the  amount  of  workspace  area  the  camera  can  observe.  The  overhead 
position  also  allows  the  camera  an  un-obstructed  and  non-interfering  view  of  the  workspace.  The  resolution 
provided  by  the  camera  is  lpixel/cm2  with  an  overall  workspace  area  of  ss  7.7m2. 


Figure  4.3:  The  mounting  bracket. 


1.2  Image  processing 

The  identification  scheme  designed  for  the  robots  consists  of  tagging  each  robot  with  two  blocks  of  the  same 
unique  color,  a  large  block  on  the  aft  end  and  a  smaller  block  on  the  forward  end.  The  vision  system  must 
be  able  to  differentiate  between  the  different  robot  colors  and  background  color.  Color  segmentation  is  an 
image  processing  technique  which  separates  and  sorts  the  colors  in  an  image.  Most  digital  images  contain 
three  layers  of  data:  a  red,  green  and  blue  layer1.  Each  color  layer  is  an  array  of  data  which  contains  the 
intensity  of  that  color  at  every  pixel.  When  the  three  layers  are  superimposed,  a  full  color  image  is  created. 

1  Tn  the  actual  implementation,  the  YUV  (Luminance,  Blue  Chrominance,  Red  Chrominance)  color  layer  scheme  was  used. 
This  color  scheme  was  selected  because  when  the  camera  used  this  scheme  it  captures  less  pixels  per  image  and  thus  allowed  for 
a  faster  processing  time.  Though  the  image  resolution  is  reduced,  this  did  not  significantly  effect  the  accuracy  of  the  localization. 
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Segmenting  an  image  involves  changing  a  color  intensity  layer  or  layers,  which  can  have  values  ranging  from 
0  to  255  to  a  binary  image  which  only  contains  values  of  0  and  1.  Once  an  image  has  been  segmented  to  a 
binary  image,  several  properties  can  be  extracted  by  image  processing  algorithms. 

For  instance,  creating  a  binary  image  of  only  purple  pixels  would  require  finding  all  pixels  which  have  a 
red  intensity  >  200  and  also  have  a  blue  intensity  >  200.  All  those  pixels  which  meet  the  purple  criteria 
are  set  to  1  while  all  those  which  fail  are  set  to  0.  A  binary  image  is  created  for  each  color  of  interest.  The 
vision  system  needs  to  be  calibrated  for  each  color  of  interest  and  the  lighting  conditions  of  the  workspace. 
Appendix  H  contains  a  table  of  the  color  layer  intensity  values  used  to  segment  each  of  the  colors  of  interest. 

Within  a  binary  image  an  object  is  defined  as  a  contiguous  region  of  pixels  which  have  a  value  of  1. 
Because  noise  in  a  binary  image  consists  of  objects  which  have  a  small  area,  noise  can  be  effectively  filtered 
by  only  using  those  objects  with  relatively  large  areas. 

A  filtered  binary  image  should  only  contain  two  objects  which  correspond  to  the  large  and  small  blocks 
of  color  on  the  robot.  As  long  as  the  objects’  areas  are  significantly  different,  they  can  be  processed  to 
determine  the  robot’s  position  and  orientation.  The  property  of  an  object’s  centroid  is  analogous  to  a  two 
dimensional  object’s  center  of  mass.  Thus,  this  property  is  used  for  finding  the  center  location  of  an  object. 
Since  the  larger  block  is  roughly  centered  on  the  robot,  its  centroid  is  used  as  the  position  of  the  robot  in 
the  workspace.  The  smaller  block’s  centroid  is  used  as  the  control  point  for  low  level  control  (see  Chapter  2 
Section  3).  In  addition,  a  vector  from  the  centroid  of  the  larger  block  to  the  smaller  block  determines  the 
orientation  of  the  robot. 

In  addition  to  the  identifying  the  robots,  the  vision  system  also  determined  the  location  and  edges  of  the 
obstacles.  Since  the  obstacles  are  stationary,  the  vision  system  only  needs  to  determine  their  location  at  the 
start  of  the  experiment.  In  this  project  the  obstacles  were  green  in  color  to  separate  them  from  the  robots. 

By  profiling  and  streamlining  the  MATLAB  programs  (Appendix  G),  and  rewriting  some  of  the  image 
processing  library  functions,  the  base  station  was  able  to  identify  and  orient  the  robots  in  <  .1  seconds.  This 
speed  is  fast  enough  to  allow  for  real  time  updates  of  the  robot’s  position. 

2  Communication  between  the  base  station  and  robot 

The  next  challenge  in  the  experimental  implementation  was  to  develop  a  method  for  transferring  data 
between  the  base  station  and  each  robot.  The  information  extracted  from  the  image  processing  programs 
needed  to  be  transmitted  through  a  wireless  network  to  the  robot’s  hard  drive  so  that  the  robot  could  update 
its  motion  based  on  the  new  position  information  gleaned  from  the  camera. 

Each  of  the  robot’s  PCs  have  a  wireless  Ethernet  card  which  serves  as  the  physical  layer  of  a  network 
architecture.  In  order  to  transfer  the  files  from  the  base  station  to  the  robots,  file  transfer  protocol  (FTP) 
or  HTML-based  approaches  were  considered.  Ultimately,  a  mapped  network  drive  of  the  hard  drive  of  the 
robot’s  PC  onto  the  base  station  was  the  appropriate  solution.  Using  this  approach,  the  image  processing 
or  motion  control  programs  could  write  the  extracted  data  directly  to  the  robot's  PC’s  hard  drive.  This 
approach  is  easier  to  integrate  with  a  control  program  written  in  the  C  programming  language  and  also 
provided  a  quicker  means  of  transferring  information  between  the  base  station  and  robot  than  an  FTP  or 
HTML  approach. 

The  base  station,  which  uses  the  Windows  operating  system,  requires  that  the  software  program  Samba 
be  running  on  the  Linux  PCs.  Samba  uses  the  TCP/IP  protocols  to  allow  a  Windows  PC  to  interact  with 
a  Linux  PC  as  if  the  Linux  PC  was  a  Windows  file  server  [39].  By  installing  Samba  on  the  Linux  PC,  the 
base  station,  acting  as  a  host,  was  able  to  access  the  hard  drives  of  the  Linux  PCs  as  seamlessly  as  if  the 
Linux  hard  drives  were  on  a  Windows  machine. 

Another  approach  considered  was  converting  the  base  station  into  a  Linux  machine.  However,  this  would 
require  reformatting  the  base  stations  hard  drive,  installing  and  configuring  Linux,  installing  MATLAB  for 
Linux  and  finally  networking  the  Linux  base  station  and  robot  PCs.  Linux  is  an  open  source  operating 
system  without  standardization  of  user  installation  and  configuration.  Linux  is  purposefully  non-centralized, 
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therefore  many  of  the  components  of  the  operating  system  must  be  downloaded,  installed  and  configured 
individually.  There  are  few  commercially  available  Linux  products  and  software  available.  Therefore,  open- 
source  software,  with  scant  documentation  must  be  used.  Without  substantial  experience,  these  factors  make 
it  difficult  to  install  and  correctly  configure  a  Linux  operating  system  for  a  specified  application  as  required 
by  this  project.  Even  if  all  of  the  above  steps  had  been  completed  in  a  satisfactory  and  timely  manner,  it 
is  doubtful  that  the  special  device  drivers  necessary  for  the  overhead  camera  used  in  the  project  would  run 
under  Linux. 


3  Programming  and  control  of  Koala  Robots 

The  Koala  robots  have  two  on-board  computers.  The  first  is  an  embedded  system  which  consists  of  a  small 
amount  of  flash  ROM,  a  Motorola  68K  micro-processor  and  various  robot  hardware  such  as  motor  control 
circuits,  sensors,  analog  to  digital  converters,  etc.  The  Motorola  68K  micro-processor  controls  the  motors, 
sensors,  and  internal  operation  of  the  robot.  The  flash  ROM  can  be  loaded  with  a  user’s  programs.  However, 
the  programs  on  the  flash  ROM  must  be  written  in  a  highly  specific  robot  language.  In  order  to  write  a 
program  in  the  robot  language  a  cross-compiler  is  need  to  change  a  program  written  in  the  C  computer 
language  into  the  robot  language.  [5] 

The  second  computer  that  is  on  board  the  robot  is  a  standard  Pentium,  266  MHz  personal  computer.  The 
computer  uses  a  20GB  hard  drive  and  a  wireless  Ethernet  card.  This  computer  uses  laptop-like  components 
that  are  much  smaller  than  their  desktop  counterparts  so  the  robot  can  easily  carry  the  computer.  With 
an  externally  attached  monitor,  keyboard  and  mouse,  this  becomes  a  fully  functional  PC  running  the  Red 
Hat  Linux  Operating  System  version  7.1.  Linux  was  preloaded  on  the  computer.  Because  of  the  limited 
processing  ability  of  the  computer,  Linux  is  used  because  the  operating  system  overhead  can  be  easily  limited. 
The  two  computers  on  the  robot  are  only  connected  together  by  a  serial  line. 

There  are  two  ways  for  the  user  to  control  the  robot.  The  first  is  a  compiled  method  of  control  where 
a  program  is  written  and  cross-compiled  on  the  Linux  PC.  The  program  is  then  downloaded  via  the  serial 
line  to  the  robot’s  flash  ROM.  The  robot  then  executes  this  program  until  the  program  ends  or  the  robot  is 
shut  off,  which  ever  comes  first.  A  second  way  for  the  user  to  control  the  robot  is  an  interpreter  approach. 
Simple  robot  language  commands  are  typed  into  the  Linux  PC  and  then  sent  directly  down  the  serial  line 
for  the  robot  to  execute.  Only  one  command  can  be  sent  at  a  time  and  must  be  in  the  highly  specific  robot 
language.  In  addition,  convenient  programming  constructions  such  as  loops  and  if-then  statements  cannot 
be  used. 

Since  the  interpretive  method  of  control  does  not  allow  the  robots  to  be  autonomous,  but  rather  reliant 
on  a  user  typing  commands,  the  compiled  approach  must  control  the  robots.  This  requires  the  installation 
a  cross-compiler  onto  the  Linux  PC.  The  cross-compiler  allows  a  user  to  write  programs  in  the  standard  C 
programming  language  rather  than  the  hardware  specific  and  tedious  robot  programming  language.  The 
cross-compiler  then  translates  the  C  code  into  the  robot  programming  language  which  only  then  can  be 
downloaded  and  executed  on  the  embedded  system. 

In  addition,  a  method  was  needed  which  could  allow  the  Linux  PC  to  communicate  to  the  embedded 
system  using  the  serial  line.  The  embedded  system  has  several  pre-set  modes  and  speeds  of  serial  line 
communication.  However,  two  way  communication  between  the  PC  and  the  embedded  system  could  not 
be  established.  Therefore,  compiled  programs  running  on  the  embedded  system  could  not  be  debugged  and 
troubleshot  in  an  efficient  manor. 

An  investigation  of  two  way  communication  on  the  serial  line  was  undertaken  using  the  interpreter 
control  method  because  it  provided  instant  feedback.  When  a  user  types  a  command  into  the  Linux  PC,  the 
embedded  system  executes  the  command  and  sends  a  response.  At  this  point,  a  better  use  of  the  computers 
for  control  was  designed.  A  small  C  program  could  be  written  which  would  run  on  the  Linux  PC  and  mimic 
a  user  typing  in  commands.  This  approach  streamlines  the  process  of  transferring  data  from  the  base  station 
to  the  embedded  system.  The  Linux  PC  would  act  as  a  relay  between  the  base  station  and  the  embedded 
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system.  Without  the  Linux  PC  acting  as  a  relay,  the  embedded  system  would  have  no  way  to  receive  the 
new  information  which  would  be  coming  from  the  base  station. 

Since  the  Linux  PC  does  not  have  MATLAB  and  given  the  processing  power,  it  would  be  inefficient  at 
executing  it,  all  the  potential  functions,  controllers  and  algorithms  would  need  to  be  converted  and  compiled 
into  the  C  programming  language.  Converting  these  functions  and  controllers  also  meant  that  any  subsequent 
design  or  modification  of  the  controllers  tested  and  modeled  in  MATLAB  would  require  a  new  conversion 
and  compilation  into  the  C  programming  language. 

However,  a  more  time  effective  method  of  implementing  the  controller  was  designed.  The  brute  strength 
of  the  base  station’s  processing  power  was  used  to  calculate  the  control  algorithm  for  each  of  the  robots  in 
the  experiment.  The  base  station  could  run  MATLAB  efficiently  and  therefore  the  programs  did  not  need 
to  be  converted  to  the  C  programming  language.  The  base  station  was  used  to  calculate  the  wheel  velocities 
of  the  each  of  the  robots.  It  is  important  to  note,  that  this  system  of  implementing  the  control  system  does 
not  violate  a  central  premiss  of  the  project  which  is  to  keep  control  decentralized  (Criteria  3 '.Distributed 
Operation).  While  the  base  station  does  all  of  the  calculations,  it  does  each  calculation  based  only  on  the 
information  each  robot  would  have.  Allowing  the  base  station  to  do  the  calculations  is  merely  an  efficient  use 
of  the  computing  power  available.  With  more  powerful  processors  on  board  the  robots,  or  time  to  convert 
the  programs  to  C,  there  is  absolutely  no  reason  why  the  control  calculations  could  not  be  performed  on 
board  the  robots.  Appendix  J  contains  the  MATLAB  computer  code  for  these  sections. 

The  robots  themselves  still  run  a  small  C  program  running  on  the  Linux  PC  to  collect  and  transmit  the 
calculated  wheel  velocities.  Once  the  base  station  had  finished  calculating  the  wheel  velocities  for  a  robot, 
these  values  are  written  to  a  file  on  the  robot  hard  drive  (as  discussed  in  Section  2).  The  small  C  program 
running  on  the  Linux  PC  reads  this  file  and  writes  the  values  down  the  serial  line  to  the  embedded  system. 
The  embedded  system  takes  the  wheel  velocity  values  and  translates  them  into  motor  voltages  (Figure  4.4). 


Figure  4.4:  The  robot  hardware. 


A  timing  file  was  used  in  order  to  synchronize  the  communication  between  the  robot  and  the  base  station. 
When  the  base  station  was  finished  calculating  the  wheel  velocities  and  writing  them  to  a  file,  the  base  station 
also  wrote  a  timing  file  on  the  robot’s  hard  drive.  The  timing  file  contained  no  data,  but  was  used  as  a  flag 
to  tell  the  small  C  program  running  on  the  Linux  PC  that  new  wheel  velocities  had  been  written.  When  the 
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C  program  was  finished  reading  and  writing  the  values  to  the  serial  line  it  would  delete  the  timing  file.  Only 
after  the  timing  file  had  been  deleted  would  the  base  station  write  new  values  for  the  wheel  velocities.  The 
timing  file  kept  both  the  Linux  PC  and  the  base  station  synchronized  so  that  only  one  of  them  was  trying 
to  the  read/write  wheel  velocities  at  a  time.  See  Appendix  I  for  the  C  code  used  to  accomplish  this. 

4  Experimental  Results 

The  most  important  result  of  the  entire  experimental  setup  was  to  prove  that  the  motion  control  algorithm 
is  efficient  enough  for  a  full  scale,  real-time  implementation.  The  velocity  information  for  a  single  robot  was 
updated  at  approximately  7  times  a  second.  This  is  well  above  the  stated  goal  of  greater  than  once  per 
second.  The  update  time  includes  the  image  processing  time  required  to  localize  the  robot,  determine  the 
velocity  based  on  the  controller,  write  the  data  and  set  the  motor  voltages.  In  an  actual  system  all  of  these 
tasks  or  variants  of  them  would  need  to  be  performed.  The  update  rate  of  7  Hz  is  an  acceptable  update  rate 
for  a  autonomous  robotic  system.  The  proof  of  concept  demonstration  substantiates  one  of  the  goals  of  the 
project  which  was  to  create  an  algorithm  efficient  enough  to  be  implemented  in  a  real-time  robotics  system 
(Figure  4.5). 

The  implementation  worked  seamlessly  in  all  aspects  except  at  the  lowest  level  of  control.  There  were 
no  bottlenecks  in  the  vision  system,  the  calculation  of  the  velocities  or  in  the  communication  between  the 
robot  and  the  base  station.  However,  the  hardware  system  had  a  difficult  time  setting  the  required  motor 
voltages.  The  requested  motor  voltages  were  within  specifications  but  the  battery  level  could  not  meet  the 
stated  voltages. 

The  batteries  consist  of  rechargeable  Nickel  Metal-Hydride  (NiMH)  cells  with  special  controlling  hard¬ 
ware.  Unfortunately,  the  batteries  have  not  been  used  extensively  for  two  years.  Unless  NiMH  are  charged 
and  discharged  regularly,  they  lose  their  ability  to  store  and  deliver  charge.  This  inability  to  hold  a  charge 
resulted  in  drastically  reducing  the  amount  of  time  the  experiments  could  be  run  and  also  the  number  of 
robots  which  could  be  used  in  an  experiment.  In  addition,  the  amount  of  motor  torque  available  is  tied 
to  the  amount  of  current  which  is  delivered  to  the  motors.  As  the  charge  level  on  the  battery  drops,  the 
available  torque  also  drops.  With  low  torque,  the  robots  had  a  difficult  time  executing  a  normal  turn  and 
so  fresh  batteries  had  to  be  swapped  into  the  robots  so  that  they  could  complete  the  turns  required  by 
the  controller.  Each  swapping  of  a  battery  required  that  the  robot  be  shutdown,  restarted  and  the  control 
programs  re-initialized. 

The  batteries  are  not  a  standard  battery  which  can  be  commercially  purchased.  New  batteries  must  be 
purchased  from  the  original  company  at  exorbitant  prices  ($  1,089).  The  batteries  contain  hardware  which 
cannot  be  reproduced  or  reverse  engineered.  However,  a  solution  to  the  battery  life  problem  was  designed 
by  replacing  the  individual  cells  within  the  battery.  This  was  not  a  factory  intended  procedure,  but  appears 
to  be  a  cost  effective  way  to  extend  the  life  of  the  batteries.  See  appendix  K  for  the  technical  specifications 
of  the  individual  cells  used  for  replacement.  The  total  cost  of  replacement  was  only  $67. 

Because  of  the  issues  of  the  battery  life,  it  took  approximately  two  hours  to  have  one  experiment  run 
to  completion.  The  experimental  time  would  not  allow  enough  experiments  to  be  run  in  order  to  collect 
meaningful  data  given  the  timeline  of  the  project.  In  addition,  because  of  the  motor  torque  issues,  it  would  be 
nearly  impossible  to  control  the  experiments  so  that  they  would  be  repeatable.  The  results  of  the  experiment 
would  be  more  dependant  on  the  charge  level  of  the  battery  rather  than  the  ability  of  the  controller  or  control 
laws. 

The  implementation  phase  of  the  project  was  completed,  delivering  the  stated  objective  of  creating  a  proof 
of  concept  demonstration  of  the  controller  on  physical  robots.  Because  of  the  battery  life  issue,  pursuing  this 
avenue  of  research  would  not  be  prudent.  Instead  a  more  in  depth  focus  on  simulation  was  explored  since 
this  would  produce  the  most  fruitful  research. 
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Figure  4.5:  Three  still  frames  from  an  experiment  showing  the  robots  coming  together  for  communication, 
avoiding  obstacles  (green  square)  and  moving  towards  the  goal  location  (yellow  square  in  frame  3). 
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Chapter  5 

Experimentation  through  Simulation 


As  a  large  swarm  of  robots  move  about  the  workspace,  the  control  laws  presented  earlier  cause  the  robots  to 
maintain  line  of  sight  and  range  to  other  robots.  When  both  of  those  conditions  are  met  a  communication 
link  can  be  established.  When  multiple  links  are  created,  the  swarm  as  a  whole  constitutes  a  mobile  wireless 
communication  network.  The  number  of  robots  needed  to  illuminate  trends  in  the  properties  of  these 
networks  and  the  number  of  experimental  trials  required  to  ensure  the  results  were  statistically  significant 
precluded  hardware  based  experimentation.  In  this  chapter  large  scale  simulations  (10+  robots)  are  used  to 
understand  the  emergent  swarm  wide  behavior  under  the  influence  of  the  control  laws  presented  earlier. 

Since  robustness  is  frequently  cited  as  a  major  motivation  to  employ  swarm  robotics,  the  robustness  of 
the  resulting  network  was  then  analyzed  using  various  connectivity  metrics  from  mathematical  graph  theory. 
The  connectivity  metrics  of  the  swarm  can  be  used  to  quantify  the  redundancy  of  the  communication  network 
with  respect  to  the  failure  of  individual  robots.  Such  an  analysis  was  one  of  the  promised  deliverables  of  the 
project.  In  this  chapter  we  also  explore  the  tradeoff  between  redundancy  and  task  completion  as  measured 
by  the  time  required  for  each  robot  to  reach  its  goal. 

1  Modeling  robot  swarms  as  graphs 

A  mathematical  graph  Q  consist  of  vertexes  V  (also  called  nodes)  and  edges  £ .  For  convenience  in  discussion, 
the  graphs’s  n  vertices  are  labeled  V  =  {1,2, ...  ,  n}  although  the  labels  themselves  are  arbitrary.  An  edge 
is  essentially  an  unordered  pair  of  vertices  e  =  (i,j)  with  i,j  G  V  and  e  £  £. 

A  graph  with  n  nodes  can  be  represented  numerically  by  an  n  by  n  matrix  called  a  connectivity  matrix, 
C .  If  nodes  i  and  j  are  connected  by  an  edge  then  Cij  =  1.  C).,  =  0  by  convention.  C  is  symmetric  along 
the  main  diagonal.  Using  this  matrix,  several  numerical  measures  of  the  graph  can  be  determined. 

The  swarm  can  be  modeled  as  a  graph.  Each  robot  is  a  vertex.  An  edge  exists  between  two  vertices 
(robots),  if  and  only  if  both  range  and  line  of  sight  are  established.  An  edge  represents  a  communication  link 
(Figure  5.1  and  Figure  5.2  a).  Note  that  the  graph  theoretic  representation  does  not  encode  any  specifics 
about  the  positions  or  velocities  of  the  robots  or  obstacles.  It  is  simply  an  abstract  representation  which 
models  the  connection  topology  of  the  swarm. 


2  Graph  theoretic  measures  of  connectivity 

A  graph  is  said  to  be  connected  if  and  only  if  there  exists  a  path  from  every  node  to  every  other  node.  If  a 
swarm  is  connected  a  single  communication  network  exists.  If  the  graph  is  disconnected,  several  connected 
sub- networks  may  exist. 
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Figure  5.1:  A  communication  network  created  by  5  robots  in  a  simulation. 


Figure  5.2:  a)  A  graph,  Q  which  has  5  nodes  and  7  edges,  b)  K(Q)  >  1  because  the  removal  of  any  node  does 
not  disconnect  the  graph,  c)  K(Q)  =  2  because  with  the  removal  of  2  nodes  the  graph  becomes  disconnected. 


The  measure  of  connectivity  which  this  project  is  most  interested  in  is  the  K- connectivity,  K(Q).  The  K 
value  is  the  minimum  number  of  nodes  which  when  removed,  disconnect  the  graph.  If  a  graph  is  connected 
and  removing  one  node  will  disconnect  the  graph  then  the  graph’s  K  value  is  1  (also  called  a  1-connected 
graph).  If  it  takes  two  nodes  to  be  removed  before  the  graph  becomes  disconnected  then  the  K  value  is  2, 
etc.  The  K-connectivity  is  a  worst  case  scenario  measure,  it  represents  the  minimum  number  of  nodes  which 
must  be  removed  before  the  graph  becomes  disconnected  (Figure  5.2). 

This  property  is  of  special  interest  to  the  project  because  as  Criteria  4 -.Fault  Tolerance  states,  the  con¬ 
troller  should  be  able  to  survive  a  massive  communication  disturbance  such  as  the  loss  of  a  node  (robot). 
The  K  value  explicitly  defines  how  many  nodes  (robots)  can  be  lost  before  the  swarm  no  long  is  connected. 
In  order  to  for  a  network  to  truly  be  fault  tolerant,  it  must  be  at  least  2-connected,  implying  the  swarm’s 


50 


connectivity  hinges  on  no  single  robot. 

However,  four  drawbacks  make  K  insufficient  as  a  stand-alone  measure  of  network  robustness  or  redun¬ 
dancy. 

1.  Most  importantly,  no  motion  controller  that  satisfies  the  project’s  design  Criteria  3 '.Distributed  Oper¬ 
ation  can  directly  influence  K,  because  computing  K  requires  knowledge  of  every  robot’s  position.  K 
is  a  global  measure. 

2.  K  is  an  overly  conservative  measure  of  connectivity.  It  represents  a  worst  case  scenario  -  the  minimum 
number  of  vertex  failures  which  disconnect  the  swarm.  Two  swarms  can  have  identical  K  values, 
however,  one  of  the  swarms  may  have  many  more  edges  making  it  much  more  robust  to  single  robot 
failure. 

3.  The  connectivity  of  the  swarm  varies  as  a  function  of  time  due  to  the  swarm’s  motion.  Because  K  only 
takes  on  integer  values  it  may  jump  from  K  =  2  to  K  =  3,  for  example.  This  is  another  manifestation 
of  K ’s  conservancy.  Without  some  intermediate  measure  of  connectivity  it  is  difficult  to  determine  a 
trend  line. 

4.  Finally,  K  is  difficult  to  compute  because  it  requires  a  series  of  depth-first  graph  searches. 

For  these  reasons  five  additional  metrics  were  used  to  determine  the  overall  connectedness  of  the  swarm. 
They  are: 


•  minimum  degree, 

•  algebraic  connectivity, 

•  average  degree, 

•  number  of  groups  of  robots  when  the  graph  is  disconnected, 

•  percentage  of  time  in  which  every  robot  has  at  least  two  connections  (simple  redundancy) . 


Some  of  these  measures  require  further  explanation.  The  minimum  degree,  average  degree,  and  algebraic 
connectivity  are  explained  below. 

The  Degree  of  a  vertex  8{i)  is  the  number  of  edges  incident  on  that  vertex.  In  the  case  of  robot  swarms, 
this  is  the  number  of  communication  links  a  given  robot  i  has  established  at  any  given  time.  This  can  be 
computed  from  the  matrix  C  by 

n 

3  =  1 


The  desired  degree  of  a  given  vertex  is  important  because  the  minimum  acceptable  value  for  the  number 
of  communication  links  for  a  certain  robot  can  be  easily  specified  as  an  input  to  the  parallel  composition 
controller. 

The  Minimum  Degree 

n 


min 


is  very  important  because  by  Whitney’s  inequality  ([17,  page  43])  and  theorem  6.8  of  [4]: 


max(0, 25min(<?)  +  2  -  n)  <  K{Q )  <  £min(0) 

(max(0, 2Smin(Q)  +  2  —  n)  can  be  abbreviated  as  K).  Thus,  the  minimum  degree  provides  upper  and  lower 
bounds  on  the  K-connectivity.  Note  however  that  these  bounds  are  highly  conservative.  It  is  also  of  interest 
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because  by  assigning  each  robot  a  desired  minimum  acceptable  degree,  one  can  directly  control  the  swarm’s 
K-connectivity  in  a  distributed  fashion. 

Another  property  that  can  be  extracted  from  C  is  the  Algebraic  Connectivity  [12],  e?  (<?).  It  is  equal  to 
the  second  smallest  eigenvalue  of  the  Laplacian  matrix  of  C.  There  are  several  features  of  this  property 
which  make  it  attractive  to  use.  First,  e-i  (Q)  =  0  if  and  only  if  the  graph  is  disconnected.  According  to  [15] 

e2(0)  <  K(G). 

Therefore,  this  sets  a  lower  bound  on  the  value  of  K.  K  is  a  worst  case  scenario  measure  and  only  moves  in 
integer  steps.  More  edges  can  be  added  to  a  graph,  but  the  K  value  will  not  necessarily  increase.  However, 
the  value  of  e2  can  be  any  real  number  and  follows  the  number  of  edges.  Thus,  e2  provides  a  less  conservative 
measure  of  the  overall  interconnectedness  of  a  graph.  It  is  also  very  convenient  to  compute.  It  can  be  shown 
that  the  algebraic  connectivity  is  related  to  the  Average  Degree 

1  n  n 

Savg(G)  =  ~  ^2  ^2 
i=\  i= 1 

Thus  in  summary,  while  the  K  value  is  the  primary  interest  in  this  project,  it  cannot  be  set  directly 
and  is  an  overly  conservative  comparative  measure  of  robustness.  Other  measures  of  connectivity  are  more 
telling.  The  two  most  important  are  the  minimum  degree  and  the  algebraic  connectivity.  The  minimum 
degree  can  be  set  at  a  desired  value  by  changing  the  desired  number  of  communication  links  per  robot  in  the 
parallel  composition  algorithm  subroutine.  The  minimum  degree  indirectly  effects  K  by  setting  upper  and 
lower  bounds  on  its  value.  The  algebraic  connectivity  is  continuous  and  proportional  to  the  number  of  edges 
in  the  graph.  It  is  highly  convenient  to  measure  and  provides  a  tighter  lower  bound  on  K.  These  measures 
are  related 

max(0, 2Smin(Q)  +  2  -  n)  <  e2(Q)  <  K{Q)  <  <Smi„(£/). 

Furthermore  since  K  is  always  an  integer  and  e2  is  a  real  number,  e2  can  be  rounded  up  to  give  an  even 
tighter  bound. 

As  an  example,  the  graph  in  Figure  5.2  a  has  values  of  K  =  1,  e2  =  1.5858,  K  =  2  and  <5min  =  2. 


3  Experimental  procedure 

A  series  of  simulations  were  run  to  determine  how  changing  the  number  of  desired  links  per  robot  affects 
the  K  value.  In  the  experiments,  the  desired  number  of  links  per  robot  (M)  was  the  independent  variable 
and  the  six  metrics  listed  above  were  the  dependant  variables.  Other  variables  were  controlled  to  provide  a 
meaningful  result.  All  simulations  used  the  same  set  of  30  different  workspaces.  Each  workspace  was  a  500 
by  500  centimeter  box  with  two  obstacles  and  15  robots.  The  robots  were  placed  randomly  within  a  200 
by  200  cm  box  in  the  lower  left  hand  corner.  Each  robot  was  assigned  a  unique  goal  position.  These  goal 
positions  were  randomly  placed  in  a  100  by  100  centimeter  box  in  the  upper  right  hand  corner.  One  obstacle 
was  a  quadrilateral  while  the  other  was  a  triangle.  The  obstacles  were  placed  by  hand  to  ensure  variety 
in  the  workspace  composition.  In  some  workspaces  the  obstacles  were  placed  in  the  direct  line  of  the  goal, 
while  in  other  a  narrow  passage  existed  in  direct  line  to  the  goal  and  in  some,  the  obstacles  were  placed  so 
they  wouldn’t  interfere  too  much  with  the  robots  moving  towards  the  goal.  Figure  5.3  shows  some  sample 
workspaces  while  Figure  5.4  is  simulation  in  progress. 

A  desired  minimum  degree  was  set  at  the  start  of  a  experiment.  Thirty  simulations,  one  for  each 
workspace,  would  be  run  for  1000  iterations  each.  At  each  iteration  the  metrics  were  checked  to  determine 
robustness  of  the  swarm  communication  network.  At  the  end  of  an  experiment,  six  30  by  1000  matrices  of 
data  points  were  created.  The  data  was  then  averaged  over  the  30  runs.  Experiments  were  conducted  on 
both  controllers  with  zero  through  eight  desired  links  per  robot.  See  Appendix  N  for  MATLAB  computer 
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Figure  5.3:  Four  different  workspaces.  The  x’s  mark  the  starting  positions  of  the  robots  while  the  *  are  the 
goal  positions. 
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Figure  5.4:  A  simulation  in  progress  with  the  robots  forming  a  communication  network  while  avoiding 
obstacles. 


Minimum  degree  (avg.  over  30  runs) 


Figure  5.5:  The  metrics  plotted  over  time  for  each  experiment  with  the  communication  variant. 
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Minimum  degree  (avg.  over  30  runs) 


Figure  5.6:  The  metrics  plotted  over  time  for  each  experiment  with  the  original  controller. 


code  used  to  determine  the  metrics.  Each  of  the  averaged  values  for  each  experiment  is  plotted  over  time  in 
Figures  5.5  and  5.6. 

Eight  desired  communication  links  per  robot  was  set  as  the  maximum  value  tested.  If  the  actual  number 
of  links  equals  the  desired  number  of  eight,  then  K  >  2  for  a  swarm  of  15  robots  based  on  the  value  of  K. 
This  project  is  most  interested  in  creating  a  minimum  K  value  of  2  because  it  means  that  no  single  robot 
failure  will  disconnect  the  swarm.  At  M  =  8  the  actual  number  of  communication  links  was  much  higher 
than  8  because  the  robots  were  in  such  close  proximity,  therefore,  increasing  the  number  of  communication 
links  beyond  8  would  not  produce  anything  more  meaningful. 

The  experiment  with  M  =  0  simulates  a  swarm  in  which  each  robot  moves  independently,  with  no 
regard  for  swarm  wide  networking.  This  was  used  as  the  baseline  to  determine  how  much  of  a  difference  the 
controllers  made  in  establishing  a  communication  network.  As  the  robots  move  around,  and  especially  given 
the  workspace  set  up,  some  communication  links  will  be  established  by  happenstance.  The  baseline  results 
provide  a  measure  of  how  robust  a  natural  communication  network  will  be  when  each  robot  is  independently 
moving  towards  the  same  general  area.  The  workspaces  could  have  been  carefully  constructed  so  that  no 
communication  links  would  form  by  happenstance.  However,  this  is  not  necessary,  all  that  is  necessary  is 
that  a  baseline  is  established  and  then  to  determine  the  amount  and  degree  to  which  the  controller  deviates 
from  that  baseline. 


55 


4  Experimental  results 

It  was  predicted  that  the  more  connections  desired  per  robot,  the  greater  the  robustness  of  the  swarm  but  also 
the  longer  it  would  take  the  swarm  to  reach  its  goal.  In  addition,  it  was  believed  that  the  robustness  of  the 
swarm  would  hit  a  saturation  level  and  increasing  the  desired  number  of  links  would  not  effect  the  robustness 
after  that  saturation  level.  Additionally,  it  was  predicted  that  the  communication  variant  would  establish 
a  stronger  network  at  the  price  of  increased  time  to  goal.  While  these  were  the  qualitative  predictions,  no 
educated  quantitative  prediction  could  be  made. 

When  examining  the  data,  some  of  the  metrics  do  not  provide  much  useful  information.  The  measure 
of  simple  redundancy  increased  until  it  reached  saturation  at  M  =  2.  The  number  of  groups  also  did  not 
provide  much  useful  information,  for  all  of  the  experiments  where  M  ^  0  the  number  of  groups  quickly 
converged  to  1. 

For  the  meaningful  metrics,  the  data  was  averaged  over  the  first  400  iterations.  The  first  400  iterations 
were  chosen  because  by  this  point,  in  all  experiments,  the  robots  had  settled  to  steady  state  values.  This 
project  is  more  interested  in  the  transient  response  of  the  controller,  i.e.  the  response  of  the  controller  as 
the  robots  move  around.  The  averages  for  this  data  are  shown  in  Figure  5.7.  In  addition,  the  data  for  the 
average  time  to  goal  is  also  plotted.  A  better  interpretation  of  the  data  is  based  on  the  percentage  increase 


Comparison  of  original  and  comm,  variant  controller 


Figure  5.7:  The  values  for  the  connectivity  and  time  to  goal  metrics  for  every  experiment.  Triangles  mark  up¬ 
per  and  lower  bounds  for  K-connectivity  of  the  Comm,  variant,  while  *  mark  the  the  upper  and  lower  bounds 
of  the  original  controller’s  K-connectivity.  Note  the  different  y  axes  for  time  to  goal  and  K-connectivity. 
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in  the  various  values.  The  baseline  results  are  simply  a  result  of  the  workspace  design;  therefore,  it  is 
more  meaningful  to  examine  the  percentage  increase  rather  than  the  absolute  values  of  the  data.  Therefore, 
Figure  5.8  is  a  comparison  of  the  percentage  increase  in  robustness  and  time  to  goal  for  all  the  experiments. 
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Figure  5.8:  The  percentage  difference  in  the  metrics  between  the  baseline  and  the  various  experiments. 

Only  demanding  each  robot  to  have  one  communication  link  did  little  to  affect  the  robustness  of  the 
swarm  or  the  time  to  goal.  It  appears  that  the  controllers  reach  saturation  for  values  of  M  >  n/2  but 
without  experimental  data,  this  cannot  be  proven  conclusively.  Also  as  predicted,  the  communication  variant 
consistently  established  a  more  robust  network  than  the  original  or  goal  controller.  The  communication 
variant  had  a  larger  impact  when  the  number  desired  number  of  communication  links  was  larger.  However, 
this  increased  robustness  cost  the  swarm  an  approximate  20%  increase  in  the  time  to  reach  the  goal. 

It  is  also  interesting  to  note  that  while  the  conservative  lower  bound  is  at  zero  for  most  of  the  experiments, 
the  e2  metric  is  consistently  higher  than  the  conservative  lower  bound.  This  means  that  the  controller  is 
consistently  pushing  the  connectivity  up  towards  a  higher  value.  In  some  cases,  <5  >  M .  This  happens  because 
as  the  robots  pull  themselves  into  a  communication  network,  other  communication  links  are  established  by 
the  fact  that  the  robots  are  now  in  closer  proximity. 

When  taken  as  a  whole,  the  controllers  developed  in  this  project,  can  increase  the  robustness  of  a 
communication  network  at  least  200-  300%  while  only  costing  a  25-45%  increase  in  the  time  to  accomplish 
the  goal.  Depending  on  if  the  mission  is  time  critical  or  if  it  needs  good  communication,  the  controller  could 
be  tuned  to  meet  the  demands. 
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4.1  Sources  of  error 

The  data  is  averaged  in  each  of  the  experiments.  Thus,  few  guarantees  can  be  made  about  the  performance 
of  an  individual  swarm,  only  average  statements  can  be  made.  Given  another  workspace,  the  swarm  could 
perform  below  its  average  for  its  given  controller.  This  controller  is  dependent  on  the  initial  conditions  of 
the  workspace  and  how  that  affects  communication  links  being  created  by  happenstance.  By  analyzing  the 
percentage  increase  over  the  baseline,  the  effect  of  the  initial  conditions  should  be  eliminated. 

K  reflects  the  relationship  between  M  and  a  lower  bound.  However,  this  relation  is  very  conservative 
and  does  not  provide  an  accurate  reflection  of  the  K  values  in  a  simulation.  A  more  accurate  formula  which 
relates  Q  to  a  lower  bound  of  K  is  desired.  The  network  created  is  a  special  type  of  graph  called  a  proximity 
graph  so  perhaps  by  using  the  properties  of  this  type  of  graph,  a  tighter  theoretical  constraint  on  K  could 
be  established.  Time  limits  prohibit  this  investigation. 
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Chapter  6 


Conclusion  and  Future  Work 


1  Conclusion 

This  project  designed  a  solution  to  the  stated  task  of  creating  an  overall  motion  control  algorithm  which  can 
accomplish  the  dual,  and  sometimes  conflicting  requirements  of  executing  a  primary  mission  while  at  the  same 
time  establishing  and  maintaining  a  robust  communication  network.  This  project  created  several  potential 
fields  to  accomplish  this,  each  of  which  is  an  original  theoretical  contribution.  Additionally,  a  novel  high 
level  control  algorithm  was  created  and  implemented.  The  parallel  composition  scheme  took  a  theoretical 
(nonconstructive)  proof  and  exploited  the  planar  structure  of  the  problem  to  develop  a  computationally 
efficient:  1.  feasibility  test  and  2.  method  of  computing  velocity  vectors. 

Additionally,  two  different  algorithms  were  developed  which  prioritize  and  determine  a  feasible  set  of 
vectors.  Both  allow  the  most  urgent  and  important  vectors  to  be  reflected  in  the  final  velocity.  One  is 
optimized  for  communication  and  the  other  for  goal  completion. 

Several  hundred  (510)  simulations  were  run  to  test  and  analyze  the  performance  of  the  controller  at 
accomplishing  the  goal  and  maintaining  a  robust  communication  network.  The  results  of  the  simulations 
prove  that  the  controller  is  able  to  accomplish  its  dual  objectives  simultaneously.  Quantitative  analysis 
shows  that  using  the  controller  can  increase  the  connectivity  of  the  swarm,  as  compared  to  the  baseline 
experiments,  by  200-300%  while  only  incurring  a  25-45%  increase  in  the  time  it  takes  to  reach  the  goal. 

The  controller  was  also  implemented  on  a  physical  robot  system  to  test  how  the  controller  could  be 
implemented  on  real  robots.  Even  with  all  other  component  necessary  for  the  robot  system,  the  algorithm 
was  fast  enough  that  it  could  update  a  robot’s  speed  7  times  a  second.  This  proved  that  the  algorithm  was 
efficient  enough  to  be  used  as  a  real-time  controller  for  robots. 

The  functions,  algorithms,  and  controllers  created  in  this  project  accomplish  the  task  goals  in  way  that 
was  designed  for  swarms  of  multiple,  unsophisticated,  independent  robots.  The  functions,  algorithms  and 
controllers  accomplish  criterions  of  Goal  Completion  and  Robust  Communications  in  a  manner  that  satisfied 
the  criterions  of  Distributed  Operation  and  Fault  Tolerance. 

In  summery,  the  accomplishments  of  this  project  are  as  follows: 

Theoretical  accomplishments 

•  Designed  a  distributed  control  law  for  maintaining  inter-robot  separation  between  two  robots.  This 
control  law  allows  for  the  minimum  separation  distance  and  maximum  communication  distance  to  be 
set  independently. 

•  Designed  a  distributed  control  law  for  maintaining  line  of  sight  between  two  robots.  This  control  law 
maintains  line  of  sight  and  if  two  robots  lose  line  of  sight,  it  pushes  one  of  the  robots  out  of  the  occlusion 
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zone  and  back  into  line  of  sight  communication. 

•  An  algorithm  was  designed  which  enforces  the  redundancy  constraint  and  maximizes  the  number  of 
task  objectives  which  can  be  meet  while  minimizing  the  energy  used. 

•  An  original,  unique  and  novel  high  level  control  scheme  was  developed  which  facilitates  meeting  two 
or  more  objectives  simultaneously  and  reflects  the  importance  of  meeting  the  objectives. 

Implementation  and  design  accomplishments 

•  Designed  a  computer  vision  system  capable  of  identifying  and  orienting  the  robots  in  <  .1  seconds. 

•  Engineered  a  framework  for  controlling  the  Koala  Robots,  including  a  unique  control  methodology 
using  MATLAB,  wireless  Ethernet,  and  the  C  programming  language. 

•  Demonstrated  a  physical  system  of  robots  to  prove  that  the  control  algorithm  could  be  implemented 
and  used  on  a  real  time  robotics  system. 

Experimental  analysis  accomplishments 

•  Performed  hundreds  of  simulations  in  various  workspaces  using  large  numbers  of  robots  to  test  the 
performance  of  the  controller. 

•  Pursued  in  depth  quantitativeness  analysis  using  graph  theory  techniques  and  formulas  to  determine 
the  robustness  of  the  swarm  communication  network. 

•  Demonstrated  through  simulations  that  the  control  algorithms  can  cause  a  200-300%  increase  in  the 
robustness  of  the  swarm  while  only  incurring  a  25-45%  increase  in  the  time  to  complete  the  goal. 

2  Ideas  for  future  research 

There  are  still  many  avenues  to  extend  the  results  of  this  research.  The  experimental  K  value  is  much  higher 
than  the  theoretical  lower  bound  of  K.  Determining  a  less  conservative  relationship  between  K  and  8  is  a 
top  priority.  This  would  lead  to  a  method  of  selecting  5,  to  achieve  a  certain  degree  of  redundancy.  The 
robot’s  communication  network  is  a  special  type  of  graph  called  a  proximity  graph.  Perhaps  by  examining 
the  properties  of  these  graphs  a  tighter  relationship  could  be  found. 

One  aspect  that  this  project  explored,  but  due  to  the  time  limitations,  only  tentative  conclusions  could 
be  drawn,  is  the  idea  of  only  having  restricted  position  information.  With  restricted  position  information, 
a  robot  only  knows  the  position  information  of  the  robots  with  which  it  has  a  communication  path.  It  is 
important  to  note  that  the  motion  control  algorithm  still  functions  without  omniscience  position  knowledge. 
This  scenario  is  equivalent  to  the  ‘eye  in  the  sky’  being  removed  and  each  robot  has  to  communicate  its 
position  to  every  other  robot  with  which  a  communication  path  exists.  Not  enough  simulations  were  run 
to  provide  a  definitive  result.  However,  in  initial  simulations  each  group  of  robots  forms  the  most  robust 
communication  network  given  the  number  of  robots  and  the  Q  value.  If  by  happenstance  two  groups  should 
meet,  then  the  network  will  rearrange  itself  based  on  the  new  position  information  which  can  be  obtained.  It 
is  predicted  that,  at  the  beginning  of  a  simulation,  the  metrics  appear  to  be  similar  to  the  baseline  controller. 
At  the  start,  there  are  many  groups  and  so  there  is  little  position  information  which  constrain  the  robots. 
But  as  the  simulation  progresses,  the  groups  become  larger  and  thus  the  metrics  will  begin  to  mirror  the 
simulations  where  the  robots  had  omniscient  position  knowledge. 

A  promising  future  research  project  would  be  to  combine  this  project  with  some  of  the  work  outlined 
by  Poduri  and  Sukhatme  [30].  This  work  examines  using  proximity  graphs  for  maximizing  the  coverage  of 
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mobile  sensing  networks  while  keeping  some  degree  of  communication  robustness.  However,  the  proximity 
graphs  examined  are  a  highly  ordered  lattice  of  points  which  might  be  unsuitable  for  a  real  environment 
with  obstacles.  If  the  lattice  points  were  set  as  the  goal  positions  of  the  robots  in  an  unknown  workspace, 
then  the  controller  designed  in  this  project  could  be  used  to  guide  the  robots  towards  the  goal  positions 
while  maintaining  a  communication/sensor  network.  If  there  were  any  obstacles  in  the  way  of  the  robots 
reaching  the  lattice  points,  this  project’s  controller  would  approximate  the  ideal  lattice  positions  in  such 
a  way  that  the  communication/sensing  network  would  not  be  effected  greatly.  Thus,  using  the  results  on 
proximity  graphs  for  coverage  contained  in  [30],  and  this  project’s  motion  control  algorithm,  a  controller  for 
maximizing  sensor  converge  while  maintaining  a  robust  communication  network  can  be  created. 

To  better  approximate  the  real  world,  noise  could  be  added  into  the  communications.  This  would  mean 
that  a  communication  link,  even  though  the  robots  are  within  range  and  line  of  sight,  could  not  be  established 
for  a  brief  period  of  time.  The  problem  of  dropped  communication  links  on  the  robustness  of  a  controller  is 
closely  related  to  its  K  value.  £ (G)  is  like  the  K  value,  except  it  is  the  minimum  number  of  edges  that  need 
to  be  dropped  before  the  graph  becomes  disconnected.  The  relationship  between  K  and  £  is  as  follows  [17, 
page  43]: 

K{Q)  <  £{G)  <  6(G). 

A  final  area  of  future  exploration  is  asynchronous  communications,  where  the  robots  do  not  have  in¬ 
stantaneous  updated  information  of  the  positions  of  the  swarm  members.  Instead,  it  would  take  a  certain 
amount  of  time  for  a  robot  to  receive  updated  information  from  another  robot  on  the  opposite  side  of  the 
swarm.  This  problem  is  closely  related  to  errors  in  position  information  which  come  from  localization  errors. 
Incorporating  these  delays  and  errors  would  help  increase  the  realism  of  the  simulations. 
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Appendices 


A  Programs  for  obstacle  avoidance 

function  [dx,  dy]=  Bishopobstacle(xR,yR,  n,  stats); 

VoObstacpreprocessor  must  have  been  run  to  define  stats  and  idx 
°/0This  fucntion  returns  the  velocity  to  avoid  an  obstacle  based  upon  a 
7#  robot  ’  s 
“/position. 

°/0This  code  is  partly  based  off  of  code  written  by  PRof .  Bishop  and  Yong 
7,  Tan  in  their 
7«Trident  project. 

720JAN 

G  =  50;  7«coeffient  factor  of  potential  field, 
dc  =  15;  7«cutoff  distance 

70maxh  =  5;  70max  velocity  that  can  be  returned. 


dx  =  0; 
dy  =  0; 

7oglobal  stats 
/global  SOI 

70distocentroid  = 

°/«  distance (xR,yR, stats (n)  .Centroid(l)  , stats (n)  . Centroid(2) )  ; 

70if  (distocentroid  <=S0I  (n)  )%if  the  distance  to  the  centroid  is  less  than 
7  the  Sphere  of  Influence 


7o7o7o7on7oy.m7o7o7o7oy.nn7o7.7on 


70find  the  approprate  two  vertices 


roboangle  =  atan2(yR- stats (n) .Centroid(2) ,xR-stats(n) .Centroid(l) ) ; 


if  (roboangle  >  max (stats (n) . angles)  I  roboangle  <  min(stats (n) . angles) ) 

11  =  1; 

12  =  length(stats(n) . angles) ; 

else 

for  q  =  2 : length(stats (n) . angles) 

if  roboangle  <  stats (n) . angles (q) 

11  =  q; 

12  =q-l ; 
break 

end 

end 

end 


xl  =  stats(n) . vx(il) ; 
x2  =  stats(n) . vx(i2) ; 
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yl  =  stats(n) . vy(il) ; 
y2  =  stats(n) . vy(i2) ; 

#/«plot(xl,  yl,  ’  *0 
°/0plot(x2,  y2,  ’  g  *’) 


rarararammmmrammra: rararararammmnra 


“/finding  distances 

if  (stats(n) . vx(il)  ~=  stats(n) . vx(i2) ) 

slopeL  =  (y2-yl)/(x2  -xl)  ;  °/0L  is  short  for  line  of  obstacle. 

else 

slopeL  =  Inf 

end 


interceptL  =  -slopeL  *xl  +yl; 

A  =  slopeL; 

B  =  -1; 

C  =  interceptL; 

d  =  abs((A*xR  +  B*yR  +C)/sqrt(A*A  +B*B));  7#  based  on  point-line  distnace 
7,  formula 


angledge  =atan2(y2  -yl,x2-xl); 

angleRepulse  =  angledge  +pi/2;  7»this  works  because  of  the  way  the  angledge 
7.  is  defined  using  il  and  i2 

xL  =  xR  +cos  (angleRepulse  +pi)*d;  7othese  are  the  points  on  the  edge 
yL  =  yR  +sin (angleRepulse  +pi)*d; 


dl  =  sqrt((xL  -xl)~2+  (yL  -yl)~2) 
d2  =  sqrt((xL  -x2)~2+  (yL  -y2) ~2) 
L  =  sqrt((xl  -x2)~2+  (yl  -y2)  ~2) 


70plot(xL,  yL,  ’m  *’) 


if  (max( [dl ,d2] )  >  L)  %if  the  point  is  off  obstacele 
dvl  =  sqrt  (  (xR-xl)  ~2+  (yR  -yl)'‘2); 
dv2  =  sqrt  (  (xR-x2)  ~2+  (yR  -y2)'‘2); 

[d  indx]  =  min  ([dvl,  dv2]  )  ;  7»redefine  d 
if  indx  ==  1 ; 

angleRepulse  =  atan2(yR-yl,xR-xl)  ;  7»and  redefine  angleRepulse 
else 

angleRepulse  =  atan2(yR-y2 ,xR-x2) ; 

end 

end 


7o7o7o7.7.7o7o7o7o7.7o7o7.7o7o7.7.7.7o7o7.7.7o7o7o7o7o7o7o7o7o7.7.7o7o7o7o 


^finding  velocity 


if  d>dc 

zprime  =0; 

else 

zprime  =  G*(l/d  -1/dc) *l/d'‘2 ;  7«Latombe  pg  300  zprime  is  the  velocity 
1  field,  since  this  one  is  dif f erntable , can  take  a  short  cut 
end 


7.  if  zprime >  maxh 

7.  zprime  =maxh;  7oCapping  the  output 
7,  end 
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xunit  =  cos (angleRepulse) ;  °/0must  be  written  this  way  to  ensure  vector  is 
#/«  pointing  the  correct  way. 
yunit  =  sin (angleRepulse) ; 

dx  =  zprime*xunit ; 
dy  =  zprime*yunit ; 

7,end 
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B  Programs  for  inter-robot  separation  distance 

function  [dz]  =  Impsemi  (dist)  #/«code  written  for  speed  of  runtime  not  ease 
"/  of  readablity 

maxcom  =  9;  "/maximum  comm  range  really  80cm 
"/if  min  sep  needs  to 

"/change,  then  c,xsig  and  h  need  to  be  changed  accordingly. 

if  (dist  >  maxcom)  "/if  x  is  outside  of  max  comm  range,  use  parabola 
"/parabola 

K=.2;  “/parabolic  coeffient 
dz  =  2*K* (dist-maxcom) ; 
return 

end 
dz  =0; 
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C  Programs  for  line  of  sight 

function  [dx,dy]  =  LOSvelf(x,y,  gammab,  gammat,  outside) 

7. 

7*  figure  (2)  ;  elf  ;  hold  on; 

7,  quiver  (0,  0,  50*  cos  (gammat)  ,  50*sin  (gammat)  ,  ’r’) 

7.  quiver (0,  0,  50*cos (gammab)  ,  50*sin (gammab) ,  ’b’) 

7,  plot(x,y,  ’m  *’) 

cutoff  =.5;  7ohow  low  the  speed  should  be  at  the  occulsion  lines 
scale  =  1;  7«just  a  factor  to  scale  the  speed  by,  speed  *  1/scale 

7,  theta  =  (gammat+gammab)/2; 

7. 

7,  roboangle  =  atan2(y,x); 

intercept  =  0;  7»this  is  only  because  of  the  way  I’ve  set  it  up,  in  real 
7.  implimentation  it  needs  to  be  calced  from  other  bots  position 
7»However,  if  the  other  robot  is  set  at  0,0  and  x,y  are 
7.  adjusted 

7»accodingly ,  this  will  always  be  zero 

A  =  tan (gammab); 

B  =  -1; 

C  =  intercept; 

db  =  abs(A*x  +  B*y  +C)/sqrt(A*A  +B*B) *l/scale ;  %  based  on  point-line 
7.  distnace  formula 

A  =  tan (gammat); 

%B  =  -1; 

7oC  =  intercept ; 

dt  =  abs(A*x  +  B*y  +C)/sqrt(A*A  +B*B) *l/scale ;  %  based  on  point-line 
7,  distnace  formula 

d  =min([db  dt]  )  ;  7«find  which  occlusion  line  is  closer 

if  db  <=  dt ; 

gama  =gammab; 
slopea  =  gama  -pi/2; 

else 

gama  =gammat ; 
slopea  =  gama  +pi/2; 

end 

7»outside 

if  (outside) ; 
d  =  -d; 

end 

d  =  d  +cutof f ; 

if  d<  0 
d  =0; 

end 

7.  d; 

7,  slopea; 

7,  gama; 
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dx  =  cos(slopea)*(d) ; 

dy  =  sin(slopea) *(d) ;  °/0this  means  speed  is  proportional  to  distance  from 
*/  occlusion  line 

7,  quiver(0,  0,  50*cos (slopea) ,  50*sin(slopea) ,  ’kJ) 

7*quiver  (x ,  y ,  dx ,  dy)  ; 

70axis  equal 


function  [outside,  sanctum,  gammab,  gammat]  =  LOSconnect (x,y ,A) ; 

7.  this  function  takes  the  verticies  of  the  convex  hull  and  coverts  them 
7.  into  vectors,  it  then  finds  the  two  occulsion  vectors  in  the  same  way 
*/  that  the  smart  sum  does,  by  looking  for  a  jump  greater  than  180.  because 
7.  these  are  convex  obstacles,  the  occlusion  zone  will  always  be  less  than 
7,  180  degrees . 

y. 

I - 

“/everything  for  this  code  is  in  0  -  2*pi  radians 

*/  determine  number  of  vertices 
S  =  size(A) ; 

N  =  S  (2) ; 

theta(:,l)  =  atan2(A(2, : ) ,  A(l,:))’; 

i  =  f ind(theta<0) ; 
theta(i)  =  2*pi+theta(i) ; 

“/.  use  column  vector  ...  it  should  be  one  already? 

“/theta  =  theta  ’ 

/  now  sort  by  increaseing  theta,  ind  lets  you  reference  back  to  actual 
“/.  vectors  in  A  later 
[theta  ind]  =  sort (theta); 

"/what's  going  on  here?  ®/0everythings  okay,  sort  works  on  column  vectors 
“/theta  =  theta’ ; 

*/  looking  at  change  in  theta  as  we  go  through  list 
for  q  =1: (N-l) 

deltaTheta(q)  =  theta(q+l)  -  theta(q) ; 

end 

deltaTheta(N)  =  (2*pi-  theta(N))  +  theta(l); 
maxDtheta  =  max(deltaTheta) ; 

outside  =true;  “/assumed  outside  until  proven  otherwise 
if  (maxDtheta  ==  deltaTheta(N) )  “/if  N  is  the  largest  angle,  then  this 
*/  means  that  the  first  is  the  smallest  since  angles  are  in  increasing 
*/  order 

gammat  =  theta(N)  ;  */0no  wrap  going  on  here, 

gammab  =  theta (1) ; 
vlx  =  A(1 , ind(N) ) ; 

vly  =  A(2,ind(N));  "/find  the  constraining  vertice  points 
v2x  =  A(1 , ind(l) ) ; 
v2y  =  A(2,ind(l)); 
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roboangle  =  atan2(y,x); 
if  roboangle  <0; 

roboangle  =  roboangle+2*pi ; 

end 

if  ((gammab)  <  roboangle  &&  roboangle  <  (gammat) )  #/0this  is  an  and 
outside  =false;  #/«then  it  is  inside  occlusion  lines 
end 

else 

j  =  f ind(deltaTheta  ==  maxDtheta) ; 

'/ } wrap ’ 

gammat  =  theta(j);  '/This  is  the  angle  which  is  the  smallest  one 
gammab  =  theta(j+l) ;  °/0this  is  the  largest  angle,  however,  it  will  be 
'/  on  the  ’ bottom }  because  of  wrap 

vlx  =  A(l,ind(j));  Whence  why  it  is  called  the  gammab 

vly  =  A(2,ind(j));  "/following  the  above  convention  of  gammat  is  vl 

v2x  =  A(1 , ind( j+1) ) ; 

v2y  =  A(2 , ind( j+1) ) ; 

roboangle  =  atan2(y,x); 

if  roboangle  <0; 

roboangle  =  roboangle+2*pi ; 

end 

if  (roboangle  <gammat  I  gammab  <  roboangle)  °/,for  all  other  cases,  it 
'/  is  an  and 

outside  =false; 

end 


end 


rarararammmmmmmraranm 


°/,now  that  we  have  an  upper  and  lower  radius , 


and  upper  and  lower  occlusion 


7*  figure (2) 

7.  7»clf 
7.  hold  on; 

7,  plot ( A ( 1 ,  : ) 5  ,A(2,  :)  ’ ,  >k’)  ; 
7.  plot  (x,y, *  ~  ’ )  ; 


7o7o7o7o7o7o7o7o7o7o7o7.7oN o t e  that  we  have  converted  to  radians  from  here  on  out 
sanctum  =true ; 

if  ("outside)  °/«if  it  is  inside  the  occulsion  lines 
°/«is  it  in  the  inner  sanctium? 
slope  =  (v2y-vly)/(v2x  -vlx); 

xinter  =  (vly  -  slope*vlx) / (tan (roboangle) -slope) ; 
yinter  =  tan (roboangle) *xinter ; 

"/quiver  (0,0,  xinter ,  yinter) 

"/quiver  (0 , 0 ,  x ,  y) 

interdist  =sqrt (xinter~2  +yinter~2) ; 
robodist  =  sqrt (x~2+y~2) ; 

if(  robodist  >  interdist)  '/if  it  is  outside  of  inner  sanctum 
sanctum  =false; 

'/  it  is  not  in  the  inner  sanctum 
end 


end 
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“/in  LOS  communication  =  sanctum;  #/«it  is  only  out  of  LOS  comms  if  it  is 
°/0  inside  the  occulsion  lines  &  not  in  the  inner  sanctum! 
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D  Programs  for  redundancy 

function [dzdx,  dzdy]  =  circumcenter (x,y ,xR,yR) ; 

°/0find  the  nearest  two  bots 
for  i  =  [1 : length (xR)] 

distances(i)  =  distance(x,y,xR(i) ,yR(i) ) ; 

end 


[d  i]  =  min(distances) ; 
distl  =  d; 

11  =  i;  "/finding  shortest  distance  indicex 

distances (i)  =1000000; 

[d  i]  =  min(distances) ;  '/finding  2nd  shortest  distance 

dist2  =  d; 

12  =i ; 


xcl  =  (x+xR(il))/2 
ycl  =  (y+yR(il))/2 
xc2  =  (x+xR(i2))/2 
yc2  =  (y+yR(i2))/2 


"/finding  the  bisector  point  of  the  lines 


slopel  =  -(xR(il)-x)/(yR(il)-y)  ;  "/constructing  a  perpendicular  line 
slope2  =  -(xR(i2)-x)/(yR(i2)-y) ; 


bl  =  slopel*-xcl  +  ycl; 
b2  =  slope2*-xc2  +  yc2; 

xcircum  =  (bl-b2) / (slope2-slopel) ; 
ycircum  =  slopel*xcircum  +  bl; 

'/figure(l) 
axis  equal 
hold  on; 

plot (xcircum,  ycircum,  5  .  gJ) 
d  =  distanced, y , xcircum, ycircum) ; 

"/  distance(xR(il)  ,yR(il) ,  xcircum,  ycircum)  "/all  these  should  be  the  same 
*/  as  d 

"/  distance (xR(i2)  ,yR(i2)  ,  xcircum,  ycircum) 

K  =  1;  '/Spring  constant 


*/ z  =  .5*  K  *  d~2;  '/to  use  regular  simulation  program,  based  on 
"/  gradients,  comment  everything  after  this 

%mmmmmmmrarararararammmmram%nra 


xunit  =  (xcircum  -  x)/d;  "/this  is  a  quicker  way  to  calculate  the 
*/  gradient,  b/c  I  know  where  it  is  ending  up. 
yunit  = (ycircum  -y)/d; 

"/zprime  now  becomes  a  velocity  field,  take  dirivative  of  z 

zprime  =  K*d;  "/zprime  is  the  velocity 

dzdx  =  zprime*xunit ;  "/velocity  times  unit  vector  length 
dzdy  =  zprime*yunit ; 
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function  [dzdx , dzdy , xf c,yf c]  =  f ermatcenvel (x,y ,xR,yR) ; 

°/0this  fucntion  calculates  the  fermat  center  and  also  cacluates  the 
7,  velocity 
figure(2);  hold  on; 


rarararamy.mrararararara%rammrararamraran 


7,find  the  nearest  two  bots 


for  i  = [1 : length (xR)] 

distances(i)  =  distance(x,y,xR(i) ,yR(i) ) ; 

end 


[d  i]  =  min (distances) ; 
distl  =  d; 

11  =  i;  ^finding  shortest  distance  indicex 

distances (i)  =1000000; 

[d  i]  =  min(distances) ;  ^finding  2nd  shortest  distance 

dist2  =  d; 

12  =i ; 


^finding  angles  of  triangle  made  by  the  3  bots 
70vetrexies  are  x  =  (x,y) 

7.1  =(xR(il)  ,yR(il)  and 

7.2  =(xR(i2)  ,yR(i2) 


ax  =. . . 

acos(dot ( [xR(il)-x,yR(il)-y] , [xR(i2)-x,yR(i2)-y] )/ (norm( [xR(il)-x,yR(il)-y . . . 
] ) *norm( [xR(i2)-x,yR(i2)-y]  ) ) ) ; 

al  =. . . 

acos(dot ( [x-xR(il) ,y-yR(il)] , [xR(i2) -xR(il) ,yR(i2)-yR(il)] )/(norm( [x-xR(il . . . 
) ,y_yR(il)] )*norm( [xR(i2)-xR(il) ,yR(i2)-yR(il)] ))) ; 

a2  =. . . 

acos(dot ( [xR(il) -xR(i2) ,yR(il) -yR(i2) ] , [x-xR(i2) ,y-yR(i2)] )/(norm( [x-xR(i2. . . 
) ,y-yR(i2)] )*norm( [xR(il) -xR(i2) ,yR(il)-yR(i2)] ))) ; 


7o7p7o7o7o7o7o7o7o7o7»7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7.7o7o7o7o7o7o7o7o7o7o7o7o7o7o 

7,if  one  of  the  angles  is  larger  than  120,  fermat  point  is  at  that  angle’s 

7,vertex 

[m  index]  =  max( [ax , al , a2] ) ; 


if  (m  >=  (120* (pi/180))) 
7o  vertx  point 


7,if  it  is  very  obtuse  then  set  xfc,yfc  at  the 


if  index  ==1 
xfc  =  x; 
yfc  =  y; 

end 


if  index  ==2 

xfc  =  xR(il) ; 
yfc  =  yR(il); 
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end 


if  index  ==3 

xf c  =  xR(i2) ; 
yf c  =  yR(i2) ; 

end 


raram%%raramrarammmmmmmrararararay.m 


“/otherwise  calculate  the  fermat  center 


else 


rarararamrmrarara%mmrammmrararararay. 


'/constructing  points  on  equalateral  triangles  and  constructing  lines. 


anglel  =atan2(yR(il)-y ,xR(il)-x) ; 
angle2  =atan2(yR(i2)-y ,xR(i2)-x) ; 


if  abs(angle2-anglel)  >  pi 
if (angle l<angle2) 

pangle  =  anglel+60*pi/180;  '/if  the  difference  between  the 
'/  angles  is  greater  than  180 (or  240  more  pecisely) 

qangle  =  angle2-60*pi/180 ;  '/than  the  smaller  of  the 

'/  angles  gets  +60  added  to  it 
else 


pangle  =  anglel-60*pi/180; 
qangle  =  angle2+60*pi/180 ; 

end 


else  '/else,  if  the  difference 

'/  between  the  angles  is  less  than  180(120) 
if  (anglel>angle2) 

pangle  =  angle l+60*pi/180;  '/the  larger  of  the  angles 
'/  gets  60  added  to  it 

qangle  =  angle2-60*pi/180 ; 

else 


pangle  =  anglel-60*pi/180; 
qangle  =  angle2+60*pi/180 ; 

end 


end 


xp  =  distl*cos (pangle) +x 

yp  =  distl*sin (pangle) +y  '/finding  the  points  on  equalateral 
'/  triangles 

'/plot  (xp,yp,  *  ~  * ) 

xq  =  dist2*cos (qangle) +x 
yq  =  dist2*sin (qangle) +y 

'/plot  (xq,  yq,  O 

slopel  =  (yp-yR(i2)  )/(xp-xR(i2)  )  ;  '/constructing  lines 
slope2  =  (yq-yR(il) )/(xq-xR(il) ) ; 

bl  =  slopel*-xp  +  yp; 
b2  =  slope2*-xq  +  yq; 

g=  [0 : .5:11] ; 

plot (g, slopel*g+bl ,  ’y5) 
plot (g, slope2*g+b2 ,  ’m’) 
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xfc  =  (bl-b2)/(slope2-slopel) ;  '/solving  lines  for  intercection  pt. 
yfc  =  slopel*xfc  +  bl; 

end 


plot  (xfc,  yfc,  }  +  r’)  '/plot  fermat  center 


mmmmmmiimiimmmmm 


'/calculate  the  height  =>  potential  field 


°/0d  =  distance (x,y,xf  c ,yf c)  ;  '/calculating  potential  function 

°/K  =  1;  '/Spring  constant 

°/z  =  .  5*K*d~2 ; 


'/calculate  gradient  =>  calculate  velocity 


d  =  distance (x,y,xf c,yf c)  ;  '/calculating  potential  function 

if  d  ==0  '/in  the  case  that  on  angle  is  >120  d 
'/  =0  so  no  movement  occures,  return  to  calling  function 
zprime=0 ; 
dzdx  =0; 
dzdy  =0; 
return 

end 

K  =  1;  '/Spring  constant 

xunit  =  (xfc  -  x)/d;  '/this  is  a  quicker  way  to  calculate  the  gradient, 
'/  b/c  I  know  where  it  is  ending  up. 
yunit  =(yfc  -y)/d; 


zprime  =  K*d;  '/zprime  is  the  velocity 


if  zprime  >10 

zprime  =10;  '/velocity  cap 

end 

dzdx  =  zprime*xunit ;  '/velocity  times  unit  vector 
dzdy  =  zprime *yunit ; 
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E  Programs  for  parallel  composition  controller 

function  [feasible]  =  f easiblity (A) 

7#  this  function  takes  a  matrix  A 

7,  which  contains  a  set  of  N  2-D  vectors  concatenated  to  form  a 
7*  2XN  matrix.  Each  vector  represents  the  desired  velocity  for  one 
7,  of  the  potential  functiosn  that  are  currently  active 

7,  The  function  attempts  to  compute  a  set  of  two  basis  vectors,  such 
7,  that  any  velocity  vector  which  lies  inside  their  convex  hull  would 
7.  simulatneously  decrease  all  the  potential  functions. 

7oHere  are  two  test  examples 
7of  easible 

7.A=  [1  1  -.1  1 ;  1  2  -4  -1] 

7,  infeasible 

7.A=  [-2  1  1  -.1  1;0  1  2  -4  -1] 

7 - 


7.  determine  number  of  potential  fuctions 
S  =  size(A) ; 

N  =  S  (2)  ; 

if  N  ==1  ||  ~N;  7« it  is  feasible  if  it  is  exactly  one  or  exactly  zero 

feasible  =true; 
return 

end 


f or (i=l :N) 

7,  compute  the  angle  of  each  vector 
theta(i,l)  =  atan2(A(2,i) ,  A(1 , i) ) ’ *180/pi ; 

7,  theta  in  [0,360]  format 
if  (theta(i)<0) 

theta(i)  =  360+theta(i) ; 

end 

end 

7,  use  column  vector  ...  it  should  be  one  already? 
theta  =  theta’ ; 

7,  now  sort  by  increaseing  theta,  ind  lets  you  reference  back  to  actual 
7.  vectors  in  A  later 
[theta  ind]  =  sort (theta); 

7»what’s  going  on  here? 
theta  =  theta’ ; 
theta’ ; 

7.  looking  at  change  in  theta  as  we  go  through  list 
deltaTheta  =  [theta(l)-theta(N) ;  theta(2:N)-  theta(l :N-1)] ; 

70again  make  sure  that  deltaTheta  in  [0,360] 
f or (i=l :N) 

if  (deltaTheta(i)<0) 

deltaTheta(i)  =  360+deltaTheta(i) ; 

end 

end 

[maxDtheta,  j]  =  max (deltaTheta) ; 

if  (maxDtheta<=180) 

7o’  inf  easible  ’ 
feasible  =  false; 

else 


7«’ feasible’ 


77 


feasible  =  true; 

end 


function  A  =  paracontrollerupdate(P,C,  commsindx,  commnum)  °/0P  is  a  2-N 
*/  priority  matrix 

°/0which  is  in  the  format  of  rows:  dx;dy 

“/,C  is  a  4-N  comm  matrix  which  is  in 

°/0the  f  ormat ,  rows :  dxLOS ;  dyLOS ;  dxr ange ;  dyrange 

°/0Tom  Dunbar 
"/Trident  Project 
"/24MAR 


rarararammmmmmmraramramrammmnras 


"/Check  if  all  priority  1  velocities  are  feasible 


feasible  =  f easiblity (P) ; 

A=P ; 

while  (“"feasible)  °/0while  it  is  not  feasible 
D=  []  ; 

veloA  =[];  “/blanks  it  out  or  else  an  infinite  loop  will  occur!!! 
S  =  size(A) ; 
for  i  =  1 : S (2) 

veloA(i)  =  norm(A( : , i) ) ; 

end 


[velo  i]  =  min(veloA) ; 

D ( 1 , : )  =  [A (1,1: i— 1)  ,A(l,i+l :end)]  ;  “/delete  the  smallest  velocity 
D(2 , : )  =  [A(2 , 1 : i-1) , A(2 , i+1 : end) ] ; 
feasible  =  f easiblity (D) ;  “/recheck  feasibility 
A  =D;  “/after  the  smallest  has  been  deleted  update  A; 


“/note  that  velocities  are  sucessively  deleted,  and  thus  feasibility 
"/will  eventually  be  acheived  because  soon  there  will  only  be  two 
“/velocities  left. 

end 

°/A; 


"/A  is  now  a  2-something  matrix  which  contains  the  priority  velocities 


%y.mrararararararammmmmmmmrarararararann 


"/Now  add  a  pair(s)  of  comm  constraints 


S  =size (C) ; 

N=  S (2) ; 
for  i  =  1:N 

veloLOS(i)  =  norm( [C(l , i) ,C(2, i)] ) ; 
velorange(i)  =  norm( [C(3, i) ,C(4, i)] ) ; 
veloC(i)  =  norm([C(l,i)+C(3,i),C(2,i)+C(4,i)]); 

end 


for  i  =  commsindx 
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veloC(i)  =  Inf;  °/«if  already  in  comms,  don’t  evaluate  in  next  section. 

end 

°/0if  commnum  <2  °/*if  there  are  less  than  2  bots  in  communication  then  get 
7,  to  work 

^select  the  pair  of  comm  velocities 

for  num  =  1 :  (1-commnum)  7,do  this  once  or  twice  depending  on  how  many  bots 
7,  there  are 

[minC  i]  =  min(veloC) ;  %find  the  smallest  pair  of  comm  vectors 
while (minC  ~=  Inf) 

if  veloLOS(i)  ~=0 

feasible  =  f easiblity ( [A, [C(l , i) ; C(2, i)] ]) ;  7»is  it  feasible? 
if  (feasible) 

A  =  [A,  [C(l,i)  ;C(2,i)]]  ;  7»if  so  add  it  to  A 
end 

end 

if  velorange(i)  ~=0 

feasible  =  f easiblity ( [A,  [C(3,  i) ; C(4,  i)] ]) ;  7«is  that  range 
7,  feasible? 

if  (feasible) 

A  =  [A,  [C(3,i)  ;C(4, i)]]  ;  %if  so  add  it  to  A 
end 

end 


%A 

veloC(i)  =  Inf;  7«change  this  so  a  new  LOS  is  chosen 
[minC  i]  =  min(veloC);  7»find  the  smallest  comm  pair 
end 

end  7«ends  for  loop 

70end  7«end  if  statement  about  if  commnum  <2 
7oA 

7.  S  =  size  (A); 

7.  if  S  (2)  ==1 ; 

7.  feasibleVel  =A; 

7.  return 

7,  end 
7. 

7.  70Now  that  we  have  a  set  of  feasible  vectors  its  time  to  create  the  final 
7,  70motion  vector!  !  !  ! 

7,  feasibleVel  =  smartSum(A) ; 


function  feasibleVel  =  smartsum(A) 

7,  this  function  takes  a  matrix  A 

7,  which  contains  a  set  of  N  2-D  vectors  concatenated  to  form  a 
7,  2XN  matrix.  Each  vector  represents  the  desired  velocity  for  one 
7,  of  the  potential  functiosn  that  are  currently  active 

7,  The  function  attempts  to  compute  a  set  of  two  basis  vectors,  such 
7,  that  any  velocity  vector  which  lies  inside  their  convex  hull  would 
7,  simulatneously  decrease  all  the  potential  functions. 


7,  The  function  returns  basis  1  and 
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7.  basis2.  A  feasible  velocity  is  any  vector  beta* (alpha*basisl  + 
7,  (1-alpha) *basis2) ,  where  alpha  in  [0,1]  and  beta  in  [0,infty) 

7oHere  are  two  test  examples 
7of  easible 

7.A=  [1  1  -.1  1;  1  2  -4  -1] 

7,  infeasible 

7.A=  [-2  1  1  -.1  1 ; 0  1  2  -4  -1] 

l - 

7,  alpha  =.5; 

7.  beta  =  1; 


7,  determine  number  of  potential  fuctions 
S  =  size(A) ; 

N  =  S  (2)  ; 


7. 

7.  figure (2) 

7.  elf 
7,  hold  on; 

7.  grid  on; 

70axis([-l  1  -1  1]); 
f or (i=l :N) 

7«plots  vectors  from  potentials... 

7oplot  (  [0  A(l,i)],[0  A(2,i)]  , ’o-k’) 

7,  compute  the  angle  of  each  vector 
theta(i,l)  =  atan2(A(2, i) ,  A(1 , i) ) ’ *180/pi ; 

7,  theta  in  [0,360]  format 
if  (theta(i)<0) 

theta(i)  =  360+theta(i) ; 

end 

end 

7,  use  column  vector  ...  it  should  be  one  already? 
theta  =  theta’ ; 

7,  now  sort  by  increaseing  theta,  ind  lets  you  reference  back  to  actual 
7,  vectors  in  A  later 
[theta  ind]  =  sort (theta); 

7»what’s  going  on  here? 
theta  =  theta’ ; 

7.  looking  at  change  in  theta  as  we  go  through  list 
deltaTheta  =  [theta(l)-theta(N) ;  theta(2:N)-  theta(l :N-1)] ; 

70again  make  sure  that  deltaTheta  in  [0,360] 
f or (i=l :N) 

if  (deltaTheta(i)<0) 

deltaTheta(i)  =  360+deltaTheta(i) ; 

end 

end 

[maxDtheta,  j]  =  max (deltaTheta) ; 


if  J  ==  1 

constrl  =  N; 
constr2  =  1; 


else 


constrl  =  j ; 
constr2  =  J  —  1 ; 
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end 

7,  remember  A  was  not  sorted! 
vl  =  A( : , ind(constrl) ) ; 
v2  =  A( : , ind(constr2) ) ; 

%  now  must  find  which  side  of  nullspaces  is  the  good  part  and  compute 
basisl  =  [-l*vl (2) ; vl (1)] ; 
basis2  =  [-l*v2(2);  v2(l)] ; 

7*dot(vl,  basis2) ; 

7*dot(v2,  basisl); 

7*  if  angle  greater  than  90  shodul  flip 
if  (dot(vl,  basis2)  <0) 
basis2  =  -l*basis2; 

end 

if  (dot(v2,  basisl)  <0) 
basisl  =  -l*basisl; 

end 

7.  basisl; 

7,  basis2; 

7ofind  the  angular  difference  between  the  basisl  and  2 
anglel  =  atan2(vl(2) ,vl(l)) ; 
angle2  =  atan2(v2(2) ,v2(l)) ; 
if  anglel  <0 

anglel  =  anglel+2*pi; 

end 

if  angle2  <0 

angle2  =  angle2+2*pi; 

end 

deltangle  =  abs (anglel  -angle2)  ;  7oI’m  not  playing  any  wrap  games, 

7othe  absolute  value  of  the  difference  of  two  positive  numbers  should  be 

7.  the 

70angular  difference  between  them. 

if  deltangle  <  (pi/2)  70which  means  that  vl  and  v2  are  less  than  90 
7,  degrees  apart 

basisl  =  vl;  7,if  so,  set  the  basis  at  the  vectors  themselves 
basis2  =  v2; 

feasibleVel  =  basisl  +  basis2; 

7.  quiver(0,0,basisl(l)  ,basisl(2) ,  ’r’) 

7.  quiver (0,0, basis2(l)  ,basis2(2)) 

7. 

7.  plot([0  feasibleVel (1 , 1)]  ,  [0  feasibleVel(2, 1)]  ,  ’mo-’) 
return 

end 

magi  =  norm (basisl) ; 
mag2  =  norm(basis2) ; 

7,  this  plots  some  sample  vectors  for  visualization  purposes 

7of  easibleVel  =  beta. * (basisl*alpha  +  basis2* (1-alpha) )  ;  7«old  control  law 

7« (which  isn’t  the  original  intent  either) 

feasibleVel  =  (mag2/magl) *basisl  +  (magl/mag2)*basis2; 

7.  quiver (0,0,  (mag2/magl) *basisl (1)  ,  (mag2/magl) *basisl (2)  ,  ’r’) 

7.  quiver(0,0,  (magl/mag2) *basis2(l)  ,  (magl/mag2) *basis2 (2) ) 

7. 

7,  plot([0  feasibleVel (1 , 1)]  ,  [0  feasibleVel(2, 1)]  ,  ’mo-’) 
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F  Technical  specifications  for  FireFly2  camera 


compact,  low  cost  IEEE-1394  digital  camera 


1/4"  progressive  scan  Sony  CCD 
Compact  size  -  40x40mm 

P  1 

640x480  uncompressed  color  images 
Low  cost  OEM  board  camera  soulution 


f 

irefly2 

POINT  GREY  RESEARCH 

Firefly2  is  a  compact 
board  level  IEEE-1394 
Digital  Video  Camera. 
Firefly2  uses  a  1/4”  pro¬ 
gressive  scan  CCD  in 
order  to  stream  VGA  qual¬ 
ity  color  images  at  30  FPS 
without  compression.  The 
camera  is  provided  as 
a  complete  development 
kit  with  an  IEEE-1394 
interface  card,  cable,  and 
image  acquisition 
software. 


front  view  back  view 


Package  includes: 

•  Firefly2  board  level  camera 

•  4.5  meter,  6-pin,  IEEE-1394  cable 

•  4,  6  and  8mm  focal  length  M12  micro  lenses 

•  IEEE-1394  OHCI  PCI  Interface  card  System  requirements: 

•  PGRFIyCapture  image  acquisition  and  •  Intel  Pentium  II  or  better 

camera  control  C/C++  SDK  •  Windows  2000  or  XP 


Camera  specifications: 


Imaging  Device 

1/4”  Sony  CCD  (ICX098AK) 

Color 

VGA  640x480  format 

HAD  image  sensor  with  sguare  pixels 

Progressive  scan 

Supported  frame  rates: 

3.75,  7.5,  15  &  30  FPS 

Supported  formats: 

YtJV  4:1:1.  YIJV  4:2:2.  YUV  4:4:4.  and  RGB  24-bit 

Digital  camera  specification: 

Version  1.04 

Signal  to  noise  ratio: 

>40dB 

Connector: 

6-pin  IEFF-1394.  vertical 

Power: 

Through  IEEF-1394.  625mW  standhv.  1.25  W  active 

Brightness: 

Auto/Manual  (-3dB  to  33dB) 

Exposure: 

Auto/Mannal  (1/25s  to  1/15000s) 

Saturation: 

Manual 

White  Balance: 

Auto/Manual 

Lens  focal  length: 

4mm.  fimm  or  8mm  /included  in  the  kitt 

Footprint: 

40  x  40mm 

Weight: 

12g  with  a  micro  lens 

www.ptgrey.com 


305-1847  West  Broadway,  Vancouver,  B.C. 

Canada,  V6J  1Y6 
T:  604-730-9937  F:  604-732-8231 


Point  Grey  Research  is  a  product  engineering  and  technology  company  founded  in  January  1997. 

The  company  designs  and  develops  computer  vision  technologies  for  commercial  applications  worldwide. 
Point  Grey  Research  technology  has  been  successfully  used  in  people  tracking,  object  tracking,  modeling 
and  dimensioning,  mobile  robotics,  mining  and  many  other  computer  vision  applications. 


Continuing  product  development  is  vital  to  Point  Grey  Research.  Point  Grey  Research  reserves  the  right  to  alter  any  published  specifications  without  notice. 
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G  Programs  for  computer  vision 

"/Obstacle  Avoidence  Pre  Processing  Function 
0/«tic 

°/0gets  a  snapshot 

frame  =  getsnapshot (vid2) ; 

'/converts  it  to  a  black  and  white  image 
BW  =  frame(:,:,l)  >190  &  frame( : , : ,3)  <110; 

'/imshow(BW)  ; 

'/lables  each  region  and  finds  statistics  about  it 
[L  num]  =bwlabel(BW) ; 

stats  =regionprops(L,  ’Area’,  ’Centroid’,  ’ ConvexHull ’ ) ; 

idx  =find(  [stats  .Area]  >80)  ;  '/filters  the  output  so  only  large  areas  are 

%  shown 

bw2  =ismember (L, idx)  ;  '/extra  stuff  for  plotting 
°/0f  igure(2)  ; 
imshow(bw2) ; 
hold  on; 

°/0plot(  stuff  for  convex  hull  ) 

'/toe 

pruner  =  7 ;  '/pruning  radius 

dc  =  15;  '/cutoff  distance 
°/t  =  [0 :  .  1 :  2*pi]  ; 

for  n  =  idx  '/for  each  obstacle 


nnnm :mmrararararara%mmmrarararam%rammn 


'/find  the  sphere  of  Influence 
for  m  =1 : length(stats (n) . ConvexHull) 
distances (m) . . . 

=distance (stats (n) .Centroid(l) , stats (n) .Centroid(2) , stats (n) . ConvexHull (m, . . . 
1) , stats (n) . ConvexHull (m, 2) ) ; 


end 


SOI(n)  =max (distances)  +dc;  '/  sphere  of  influence  =  max  distance  + 
%  cutoff  distance 


%  '/plot  (SOI  (n)  *cos  (t)+stats  (n)  .Centroid(l)  ,SOI(n)*sin(t)+stats(n)  .Centroid 

l  (2) , ’r ’ ) ; 


i  m 


clear  vx;  clear  vy;  clear  Cx;  clear  Cy; clear  angles; 
vx  =stats(n) . ConvexHull ( : , 1) ; 
vy  =stats(n) . ConvexHull ( : ,2) ; 

Cx  =  stats (n) . Centroid(l) ; 

Cy  =  stats (n) . Centroid(2) ; 
count  =0; 

L  =  length (vx) -1 ; 

'/plot (vx , vy ,  ’ r  *’) 


for  i= [1:1: L] 

nnnnnnunnn 

'/prune  out  some  of  the  excess 

if(  abs (vx(i)-vx(i+l) )  <  pruner  &&  abs (vy (i) -vy (i+1) )  <pruner) 
vx(i)  =  0; 
vy (i)  =0; 


end 
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end 

vx  =  nonzeros (vx) ; 
vy  =  nonzeros (vy) ; 


7.  mmmmm%mmmmmmmmm%mmmmmmmmm% 

7.  7.7o7o7o7o 


7.find  the  angles  from  the  centroid  to  the  vetexes.  for  each  obstacle 
for  g= [1 : 1 : length (vx)] 

angles (g)  =  atan2(vy(g)-Cy ,vx(g)-Cx) ; 

end 


7o7.7o7o7o7o7o7.7o7o7o7o7.707o7o707o7o7o7o7.7o7o707o7.7o7o7.7o7o7.7o7o7o7o 


7«sort  the  angles 

anglesxy  = [angles’ ,vx,vy] ; 


anglesxy  =  sortrows (anglesxy) ;  7.puts  the  angles  in  order,  x  and  y 
7.  change  corispondingly 

stats (n) . angles  =  anglesxy (:, 1) ; 


stats(n).vx  =  anglesxy (: ,2) ; 
stats(n).vy  =  anglesxy (: ,3) ; 


7.  7o7o7.7o7o7.7o7o7o7o7.7o7o7.7o7o7.7o7o7.7o7o7o7o7.7o7o7.7o7o7.7o7o7o7o7o7o7o7.7o7o7.7o7o7.7o7o7o7o7.7o7o7.7o7o7.7o7o7.7o7o7o7o7o7o7o7.7o7o7.7o7o 


7.  mm 

plot (vx,vy, ’g  *’ ) 

end 


function  [x,  y,  orient]  =  robotpose (vid2) 

7.code  for  the  full  implimentation  starting  with  getting  a  snapshot  and 
7.outputting  the  pose  of  the  robot,  nothing  more. 

7.tic 

frame  =  getsnapshot (vid2) ; 

red  =  frame( : , : ,2)  <100  &  frame( : , : ,3)>150; 

L  =bwlabel(red) ; 

robostats  =regionpropsslick(L) ;  7.a  special  function  I  created  which  only 

7.  has  Area  and  centroid  hard  coded  into  it 
[maximus  m]  =  max ( [robostats . Area] ) ; 
robostats (m) . Area  =0; 

[maximus  n]  =max( [robostats .Area] ) ; 

orient  =  atan2 (robostats (n) . Centroid (2)  -... 

robostats (m) .Centroid(2) , robostats (n) . Centroid(l) -robostats (m) .Centroid(l) . . . 

); 

x  =  robostats (n) . Centroid(l) ; 
y  =  robostats (n) . Centroid(2) ; 

7.toc 

7. 

7o  figure  (2); 

7.  imshow(red)  ;hold  on; 
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7.  plotter  =  [robostats (n)  . Centroid(l)  ,robostats (n)  . Centroid(2)  ; 

7. 

7,  robostats(n) .Centroid(l)+30*cos(orreint) ,robostats(n) . Centroid(2)+30*sin 
7,  (orreint)]  ; 

7.  plot  (plotter  ( :  ,  1) ,  plotter  ( :  ,2) )  ; 

7.  plot  (robostats  (n)  .  Centroid(l)  ,robostats(n)  .Centroid(2)  ,  ’o’)  ; 

7.  7. 


function  robostats  =  regionpropsslick(L) ; 

7othis  was  parced  together  from  the  MATLAB  region  props  code,  only  I 
7oSelected  the  applicable  sections,  notably  got  rid  of  Parselnputs  which 
7,  was 

7«taking  the  most  time  of  any  of  the  functions. 

allStats  =  {’Area’ 

’Centroid’ 

’PixelldxList ’ 

’PixelList ’ 

’ PerimeterCornerPixelList ’ } ; 

numObjs  =  round (double (max (L( : ) ) ) ) ; 

7,  Initialize  the  stats  structure  array. 
numStats  =  length (allStats) ; 
empties  =  cell (numStats ,  numObjs); 
robostats  =  cell2struct (empties ,  allStats,  1); 

7oCompute  statistics. 

7oCompute  Area 

robostats  =  ComputePixelldxList (L,  robostats); 
for  k  =  1 : length(robostats) 

robostats (k) .Area  =  size (robostats (k) . PixelldxList ,  1); 

end 

7oCompute  Centroid 

robostats  =  ComputePixelList (L,  robostats); 
for  k  =  1 : length(robostats) 

robostats (k) . Centroid  =  mean (robostats (k) .PixelList , 1) ; 

end 


7.7.7. 

7.7.7.  ComputePixelldxList 
7.7.7. 

function  robostats  =  ComputePixelldxList (L,  robostats) 

7.  A  P-by-1  matrix,  where  P  is  the  number  of  pixels  belonging  to 
7.  the  region.  Each  element  contains  the  linear  index  of  the 
7.  corresponding  pixel. 

7.  Form  a  sparse  matrix  containing  one  column  per  region.  In 
7.  column  P,  the  location  of  nonzero  values  correspond  to  the 
7.  linear  indices  of  pixels  in  L  that  have  value  P.  For 
7.  example,  S(100,5)  is  nonzero  if  and  only  L(100)  equals  5. 
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idx  =  f ind(L) ; 
elementValues  =  L(idx); 

S  =  sparse(idx,  double (elementValues) ,  1); 

for  k  =  1 : length(robostats) 

robostats(k) .PixelldxList  =  f ind(S( : ,k) ) ; 

end 


m 

m  ComputePixelList 

m 

function  robostats  =  ComputePixelList (L,  robostats) 

7.  A  P-by-2  matrix,  where  P  is  the  number  of  pixels  belonging  to 
7#  the  region.  Each  row  contains  the  row  and  column 
7,  coordinates  of  a  pixel. 

robostats  =  ComputePixelldxList (L,  robostats); 

7*  Loop  over  each  column  of  the  sparse  matrix.  Finding  the 
7,  row  indices  of  the  nonzero  entries  in  S(:,P)  is  equivalent 
7,  to  finding  the  linear  indices  of  pixels  in  L  that  equal  P. 

7,  Convert  the  linear  indices  to  subscripts  and  store 

7.  the  results  in  the  pixel  list.  Reverse  the  order  of  the  first 

7.  two  subscripts  to  form  x-y  order. 

In  =  cell(l ,ndims(L) ) ; 
for  k  =  1 : length(robostats) 

if  ~isempty (robostats (k) .PixelldxList) 

[In{:}]  =  ind2sub(size(L) ,  robostats (k) . PixelldxList) ; 
robostats (k) .PixelList  =  [In{:}]; 

robostats (k) .PixelList  =  robostats (k) .PixelList (:, [2  1  3: end]); 

else 

robostats (k) .PixelList  =  zeros (0, ndims (L) ) ; 

end 


end 
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H  Values  for  color  segmentation 


Table  6.1:  Color  layer  intensity  values  for  color  segmentation 


Selected  color 

Y  value 

U  value 

V  value 

Red 

- 

<  100 

>  150 

Blue 

- 

>  160 

<  120 

Green 

>  190 

- 

<  110 

Orange 

>  190 

<  80 

>  120 

Purple 

>  190 

>  145 

>  135 
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I  C  program  running  on  Koala  Robot 

/♦filename:  original. c  */ 

/*  reads  speed  commands  and  writes  them  to  the  wheel  motors  */ 

/*  deletes  the  file  once  it  is  finished  with  it  */ 


#include<stdio .h> 

FILE  *  speedfile; 

FILE  *  motorcomds; 

FILE  *  dummy; 
int  speedL; 
int  speedR; 

mainO 

{ 

while (1) 

{ 

dummy  =  f open ( "/work/dummy . txt" ,  "r"); 

if ( ! dummy) 

{ 

/♦putsC'an  error  occured  while  opening  the  file  dummy");*/ 

} 

else 

{ 

f close (dummy) ; 

speedfile  =  f open ("/work/mat labfile .txt" ,  "r"); 

if (! speedfile)  /*if  it  can’t  open  !0  =  1  */ 

{ 

putsC'an  error  occured  while  opening  the  file  matlabf ile") ; 

} 

else 

{ 

fscanf  (speedfile ,  "°/0d\n70d",  &speedL,  &speedR) ; 
f close(speedf ile) ; 

/♦remove ( " /work/matlabf ile . txt " ) ; */ 
remove ( " / work/ dummy . t xt " ) ; 

motorcomds  =  f open("/dev/ttySO" ,  "w"); 
fprintf  (motorcomds ,  "D,70d,0/0d\r"  ,  speedL,  speedR); 
printf  ("D,7od,7od\n"  ,  speedL, speedR)  ; 
f close (motorcomds) ; 

> 

> 


> 

} 


J  Programs  for  physical  experiments 

“/Program  to  Run  a  implimentation  on  the  Koala  robots 
“/nothing  pretty  about  the  code,  but  it  works 

t  =0; 

tfin  =  60;  “/the  final  time 
tstep  =  .5;  “/the  time  incriment 
cyclenum  =0; 

“/Parameters 
robonum  =4; 
velscalar  =  15; 


°/0Pre  allocate  matrixes  to  imporove  computational  time 
numberof cycles  =  length ( [0: tstep: tfin] )-l ; 
connectedness  =  false ( [robonum, robonum, numberof cycles] ) ; 
positions  =  zeros ( [2, robonum, numberof cycles] ) ; 

*/  mmra%%mmmmrararamramm%mmram% 


while (t<tf in) 
clear  vel; 

cyclenum  =cyclenum+l ; 

"/Find  the  location  of  the  robots 
frame  =  getsnapshot (vid2) ; 

red  =  frame(:,:,2)  <110  &  frame  ( :  ,  :  ,3)  >150;  "/Knowlesl  N  drive 
blue  =  frame(:,:,2)  >160  &  f  rame  ( :  ,  :  ,  3)  <120;  "/0Knowles2  0  drive 
purple  =  frame(:,:,l)  >  190  &  frame(:,:,2)  >135  &  frame(:,:,3)  >125;  '/ 
7*  Knowles3  P  drive 

oragne  =  frame(:,:,l)  >  190  &  f rame ( : , : , 2)  <80  &  frame(:,:,3)  >  120; 

'/  °/Knowles5  R  drive 

"/Find  Red  position 
L  =bwlabel (red) ; 

robostats  =regionpropsslick(L)  ;  "/a  special  function  I  created  which 

'/  only  has  Area  and  centroid  hard  coded  into  it 
[maximus  m]  =  max ( [robostats . Area] ) ; 
robostats (m) .Area  =0; 

[maximus  n]  =max ( [robostats . Area] ) ; 

orient(l)  =  atan2 (robostats (n) .Centroid (2)  -... 
robostats (m) .Centroid(2) , robostats (n) . Centroid(l) -robostats (m) .Centroid(l) . 
); 


xbots(l)  =  robostats (m) .Centroid(l) ; 
ybots(l)  =  robostats (m) .Centroid(2) ; 


7*  figure (1)  ; 

7,  imshow(red)  ;hold  on; 

7,  plotter  =  [robostats(n)  .Centroid(l)  ,robostats(n)  .Centroid(2)  ; 

7* 


'/  robostats(n) .Centroid(l)+30*cos(orient(l)) , robostats (n) . Centroid(2) +30*s 
*/  in(orient (1) )]  ; 

*/  plot  (plotter  (  :  ,  1)  , plotter  (  :  ,2)  )  ; 

*/  plot  (robostats  (n) .  Centroid  (1) ,  robostats  (n)  .Centroid  (2) ,  ’o’) ; 


7“/777o7.7.7o7«7o7o7o7.7.7«7o7.“/777o7o7o7o7.7«7o7.7«7«7.7o7o7.7.7o7«7o7.77o77"/77.770/7o7o0/7.“/“/7o"/“/“/“/7o7. 


L  =bwlabel (blue) ; 

robostats  =regionpropsslick(L) ;  */a  special  function  I  created  which 

'/  only  has  Area  and  centroid  hard  coded  into  it 


[maximus  m]  =  max ( [robostats . Area] ) ; 
robostats(m) . Area  =0; 

[maximus  n]  =max( [robostats .Area] ) ; 

orient(2)  =  atan2 (robostats (n) .Centroid (2)  -... 
robostats (m) .Centroid(2) , robostats (n) . Centroid(l) -robostats (m) .Centroid(l) 
); 


xbots(2)  =  robostats (m) .Centroid(l) ; 
ybots(2)  =  robostats (m) .Centroid(2) ; 


%  figure (2); 

7.  imshow (blue ); hold  on; 

°/«  plotter  =  [robostats(n)  .Centroid(l)  ,robostats(n)  .Centroid(2) ; 

7. 


7,  robostats(n) .Centroid(l)+30*cos(orient(2)) , robostats (n) . Centroid(2) +30*s 
7*  in(orient (2) )]  ; 

7«  plot  (plotter  (  :  ,  1)  , plotter  (  :  ,2)  )  ; 

7,  plot  (robostats  (n) .  Centroid  (1) ,  robostats  (n)  .Centroid  (2) ,  ’o’) ; 


7.  7o7o7o7o7o7o7.707o7o7o7o7o7o7o7o7o7o707o7.7o7o7o7o7o7.7o7o7o7o7o7o7o7.707o7.7o7o7o7o7o7o7o7.707o7.7o7o7o707o7o7o7o7o7o7o707o7o7o7o 


L  =bwlabel (purple) ; 

robostats  =regionpropsslick(L) ;  %  a  special  function  I  created  which 

7,  only  has  Area  and  centroid  hard  coded  into  it 
[maximus  m]  =  max ( [robostats . Area] ) ; 
robostats (m) .Area  =0; 

[maximus  n]  =max ( [robostats . Area] ) ; 


orient(3)  =  atan2 (robostats (n) .Centroid (2)  -... 
robostats (m) .Centroid(2) , robostats (n) . Centroid(l) -robostats (m) .Centroid(l) 
); 


xbots(3)  =  robostats (m) .Centroid(l) ; 
ybots(3)  =  robostats (m) .Centroid(2) ; 


7«  figure  (3); 

7,  imshow  (purple)  ;  hold  on; 

7,  plotter  =  [robostats (n)  .Centroid(l)  .robostats(n)  .Centroid(2)  ; 

7. 

7.  robostats(n)  .Centroid(l)+30*cos(orient(3))  , robostats (n)  . Centroid(2) +30*s 
7.  in(orient (3) )]  ; 

7,  plot  (plotter  (  :  ,  1)  , plotter  (  :  ,2)  )  ; 

7,  plot  (robostats  (n) .  Centroid  (1) ,  robostats  (n)  .Centroid  (2) ,  ’o’) ; 

7. 


7.  7.7.7»7.7.7«7o7.7.7o7»7#7o7»7.7.7»7o7.7»7.7.7.7o7»7.7.7»7.7.7»7o7o7#7o7.7.7o7.7.7.7»7.7.7«7o7.7.7o7«7.7o7»7.7#7«7o7.7»7.7.7.7o7.7.7o7.7.7.7.7o5 

7.  L  =bwlabel(oragne)  ; 

7,  robostats  =regionpropsslick(L)  ; 


7«a  special  function  I  created  which 


7,  only  has  Area  and  centroid  hard  coded  into  it 


[maximus  m]  =  max ( [robostats .Area] ) ; 
robostats (m) . Area  =0; 

[maximus  n]  =max( [robostats .Area] ) ; 


7,  orient(4)  =  atan2 (robostats (n)  .Centroid(2)  - 

7.  robostats(m)  .Centroid(2)  .robostats (n)  . Centroid(l) -robostats (m)  .Centroid( 

7.  D); 

7,  xbots(4)  =  robostats (m)  . Centroid(l)  ; 

7.  ybots(4)  =  robostats (m)  .  Centroid(2)  ; 


7»  figure  (4); 

7,  imshow ( or agne ); hold  on; 

7,  plotter  =  [robostats (n)  . Centroid(l)  ,robostats(n)  .Centroid(2)  ; 

7. 

7,  robostats(n)  .Centroid(l)+30*cos(orient(4))  .robostats (n)  . Centroid(2) +30*s 
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#/«  in(orient (4) )]  ; 

°/0  plot  (plotter  (  :  ,  1)  , plotter  (  :  ,2)  )  ; 

°/*  plot (robostats(n)  . Centroid (1)  ,robostats (n)  .Centroid (2)  ,  ’o’)  ; 


posit ions ( 1, :, cyclenum)  =  xbots; 

positions  (2 cyclenum)  =  ybots;  '/positions  is  a  record  of  all  the 
l  positions  overtime 

"/find  the  velocity  for  each  robot 
for  r=  l:robonum 

[vel(:,r),  connectedness]  =  velcontroller (r ,  cyclenum,  xbots,... 
ybots,  connectedness,  stats,  idx) ; 

vel(:,r)  =  jacob(vel(l ,r) , vel(2,r) ,  orient(r)); 

end 

vel  =  int8(velscalar*vel) ;  '/this  maxes  out  at  127  and  -128 

flag  =true; 
while (flag) 

dummy  =  f open( ’N: Wdummy . txt ’  ,’r’); 
if  (dummy  <  0)  °/0if  dummy  isn’t  there 

speedfile  =  f open ( ’N:  Wmatlabf  ile . txt ’ ,  ’w’); 
fprintf  (speedf ile ,  ’#/d\n°/0d’,  vel (1 , 1)  , vel (2 , 1)  )  ;  '/this  is  for 
#/«  c  code 

f close (speedf ile) ; 

writedummy  =  f  open(  ’N:  Wdummy  .txt  ’  ,  ’w’); 
f close (writedummy) ; 
flag  =  false; 

end 

end 

flag  =true; 
while (flag) 

dummy  =  f  open(  ’  0 :  Wdummy  .txt  ’  ,’r’); 
if  (dummy  <  0)  °/*if  dummy  isn’t  there 

speedfile  =  fopen(’0:\\matlabfile.txt’ ,  ’w’); 
fprintf  (speedf  ile ,  ’°/d\n°/d’,  vel  (1,2)  ,  vel  (2, 2))  ;  '/this  is  for 
#/«  c  code 

f close (speedf ile) ; 

writedummy  =  f  open(  ’  0 :  Wdummy .  txt  ’ ,  ’w’); 
f close (writedummy) ; 
flag  =  false; 

end 

end 

flag  =true; 
while (flag) 

dummy  =  f  open(  ’P :  Wdummy  .txt  ’  ,’r’); 
if  (dummy  <  0)  °/*if  dummy  isn’t  there 

speedfile  =  fopen(’P: \\matlabfile.txt’ ,  ’w’); 
fprintf  (speedf  ile ,  ’°/d\n°/d’,  vel  (1,3)  ,  vel  (2, 3))  ;  "/this  is  for 
*/  c  code 


f close (speedf ile) ; 
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writedummy  =  f open( ’  P : \\ dummy . txt ’ ,  ’w’); 
f close (writedummy) ; 
flag  =  false; 

end 

end 

7.  flag  =true; 

7*  while  (flag) 

7,  dummy  =  f  open(  ’R:  \\ dummy .  txt  ’  , ’r’); 

7,  if  (dummy  <  0)  %if  dummy  isn’t  there 
7. 

7,  speedfile  =  f  open( ’R:  Wmatlabf  ile .  txt 5 ,  ’w’); 

7,  fprintf  (speedfile ,  J7od\n7od’,  vel(l,4)  ,vel(2,4)) ;  7«this  is 

7,  for  c  code 

7,  f  close  (speedfile) ; 

7. 

7.  writedummy  =  f  open(  5R:  \\dummy.txt  ’ ,  ’w’); 

7.  f  close  (writedummy)  ; 

7,  flag  =  false; 

7,  end 
7,  end 

t  =t+tstep; 

end 

vel  =  zeros ( [2,4] ) ; 
vel  =  int8(vel); 

speedfile  =  f  open( ’N:  Wmatlabf  ile .  txt  ’ ,  ’w’); 

fprintf  (speedfile ,  ,7od\n7od’,  vel  (1 , 1)  ,  vel  (2 , 1) ) ;  7»this  is  for  c  code 
f close(speedf ile) ; 

writedummy  =  f  open(  ’  N :  Wdummy .  txt  ’  ,  ’w’); 
f close (writedummy) ; 

speedfile  =  f  open( ’0 :  Wmatlabf  ile .  txt  ’ ,  ’w’); 

fprintf  (speedfile ,  ,7od\n7od’,  vel  (1,2)  ,  vel  (2, 2))  ;  7»this  is  for  c  code 
f close(speedf ile) ; 

writedummy  =  f  open(  ’  0 :  Wdummy .  txt  ’  ,  ’w’); 
f close (writedummy) ; 

speedfile  =  f open ( ’P : Wmatlabf ile. txt’ ,  ’w’); 

fprintf  (speedfile ,  ’7od\n7od’,  vel  (1,3)  ,  vel  (2, 3)) ;  7»this  is  for  c  code 
f close(speedf ile) ; 

writedummy  =  fopen(  ’P:  Wdummy  .txt  ’  ,  ’w’); 
f close (writedummy) ; 

7,  speedfile  =  f  open(  JR:  Wmatlabf  ile .  txt  * ,  Jw’); 

7,  fprintf  (speedfile ,  ’7od\n7odJ,  vel(l,4)  ,vel(2,4)) ;  7»this  is  for  c  code 
7,  f close(speedf ile) ; 

7. 

7.  writedummy  =  fopen(  ’R:  Wdummy .  txt  ’  ,  ’w’); 

7.  f  close  (writedummy)  ; 
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function  [vel,  connectedness]  =  velcontroller (r ,  cyclenum,  xbots,  ybots,... 
connectedness,  stats,  idx) 

#/oTom  Dunbar 
7.5FEB 

°/0this  program  sets  up  the  velocities  necessary  to  calculate  a  robot’s 
°/0new  velocity  and  then  passes  the  information  on  to 


7  Parameters777777777777777777777777 

robonum  =  4;  °/0the  number  of  robots  in  the  simulation 
gscale  =20;  °/0goal  scaler,  note  that  l/gscale*gdx 
oscale  =25;  “/obstacle  avoidence  scaler,  oscale*oadx 
°/0maxvel  =  5;  “/maximum  velocity 
minsep  =  50;  '/this  is  devided  by  10; 
gx  =  300; 
gy  =  220; 


P  =  [0 ;  0]  ; 

C  =  zeros (4, robonum) ; 


“/Goal  veiocity77m7%77rammmrammm%%mm 

gdx  =  l/gscale*(gx-xbots(r) ) ; 
gdy  =  l/gscale*(gy-ybots(r) ) ; 

rara%mmmmraramrara%mmmm%ra 


if  (sqrt ( (gx-xbots (r) ) ~2+  (gy-ybots  (r)  )  "2)  <  10  )  '/ if  robot  is  near  the 

7  goal,  do  nothing, 

’robot  at  goal’ 

[vel,  connectedness]  =  paracontroller(P,C,r , cyclenum,  connectedness); 
7  “/just  so  the  connectedness  matrix  gets  filled  in 
return 

else 

P( : , 1)  =  [gdx; gdy]; 

end 


for  o  =  idx  “/for  each  obstacle 


“/Obstacle  avoidanc e77777777777777777777777777777777 

[oadx  oady]  =  Bishopobstacle (xbots (r) ,  ybots (r),  o, 

777777777777777777777777777777777777777777777777777 


stats) ; 


if (sqrt (oadx'‘2+oady'"2)  ~=  0)  “/if  there  is  a  vector,  pass  it  to  the 
7  controller 

P(:,2)  =  [oadx; oady] . *oscale ; 

end 


7.L0S  stuff 7o77777o777o777o77777777o777o777o77777o7777777777 
Co  =  zeros (4, robonum) ; 

for  q  =1:  robonum  “/check  LOS  with  each  robot 


if  q  ’=  r  “/the  robot  will  always  have  LOS  with  itself... 


A  =  [stats(o) .vx’ ; stats(o) . vy’] ; 

xB  =  xbots (q); 
yB  =  ybots (q); 

Ab(l,:)  =  A ( 1 ,  : )  -  xB;  “/translating  the  obstacle  so  B  is  at 


7,  the  origin,  do  it  can  be  checked  by  LOSchecker 
Ab(2 , : )  =  A(2 , : )  -  yB; 


xtrans  =  xbots(r)  -xB; 
ytrans  =  ybots(r)  -yB; 

[flag,  outside,  gammab,  gammat]  =  LOSchecker (xtrans , ytrans ,.. . 

Ab)  ; 


if  (flag) 

[L0Sdx,L0Sdy]  =  LOSvelf (xtrans , ytrans ,  gammab,  gammat,... 
outside) ; 


Co (1 ,q)  =  LOSdx; 

Co (2 ,q)  =  LOSdy ; 

end 

end  70end  if  statement 

7.7.7.7.7»7o7.7«7o7#707o7»7.7o7»7.7.7«7oy.7«7o7.7.7oy»7.7«y.7.7#7»7o7.7.7oy.7.7o7«7.y. 


end  °/0end  for  each  robot  q 


for  c  =  l:robonum 

if  (abs(C(l,c))  <  abs(Co(l,c))) 

C(l,c)  =  Co(l,c);  °/0this  replaces  a  previous  LOS  vector 
end 

if  (abs(C(2,c))  <  abs (Co (2 , c) ) ) 

C(2 , c)  =  Co (2 , c) ; 

end 

end 


end  °/0end  for  each  obstacle 


°/0Range 

for  q  =  l:robonum  °/0for  each  robot 
if  q~=r 

dist  =  sqrt((xbots(q)-xbots(r))'"2  +  (ybots  (q)  -ybots  (r)  )  ~2)  ;  °/0dist  =  0 
#/«  if  they  are  the  same 


xunit  =  (xbots(q)  -  xbots  (r) ) /dist ;  "/.must  be  written  this  way  to 
#/«  ensure  vector  is  pointing  the  correct  way. 
yunit  =  (ybots (q)  -  ybots (r) ) /dist ; 


if  (dist  &&  dist  <  (minsep))  °/0dist  must  be  nonzero  and  less  than 
7,  minsep 

delta  =.01; 
h  =.556929; 

xsig  =1.1229*h;  °/0these  values  based  off  of  a  valley  at  2.5 
c  =  7.05538; 

°/0mexhat 

dist  =  dist/minsep;  #/0this  changes  distance  to  a  value  from  0  to  1 


zplus  = .  .  . 

(l/(2*pi*xsig) )*c* (h- (dist+delta) ~2/xsig~2) *exp(-l* (dist+delta) ~2/(2*xsig 

2)); 


zminus  = . . . 

(l/(2*pi*xsig) )*c* (h- (dist-delta) ~2/xsig~2) *exp(-l* (dist-delta) ~2/(2*xsig 

2)); 


dz  =  (zplus-zminus)/(2*delta) ; 
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P(l,end+1)  =  dz*xunit; 

P(2,end+1)  =  dz*yunit ;  °/«if  the  distance  is  less  than  minimum 
7.  seperation, 
else 

dz  =  Impsemi(dist/10) ; 

C(3,q)  =  dz*xunit;  ^velocity  times  unit  vector 
C(4,q)  =  dz*yunit; 

end 

end 


end 


rara%mmmmrararararara%mmmraran 


%  [M  N]  =  size(P); 

7*  for  p  =  1:N 

7,  quiver  (xbots  (r)  ,ybots  (r)  ,P(1 ,p) ,P(2 ,p) , ’b’ ) 

7o  end 


7.  [M  N]  =  size(C); 


7.  for  c=l:N 

7.  quiver  (xbots  (r)  ,ybots(r)  , C(1 , c) ,C(2 , c) , ’g’ ) 
7.  quiver  (xbots  (r)  ,ybots  (r)  ,C(3, c) ,C(4, c) , ’r ’ ) 
7,  end 


7.  P 

7.  c 


[vel,  connectedness]  =  paracontroller(P,C,r , cyclenum,  connectedness); 
7»f  igure(l) 

70quiver (xbots (r)  ,ybots(r)  ,f easibleVel (1)  ,f easibleVel(2) ,  ’m’) 

7,  dmag  =  norm(f easibleVel) ;  7othis  stuff  should  be  taken  care  of 
7.  in 

7.  7*the  int8  convert  ion 
7. 

7,  if  (dmag  >maxvel) 

7,  feasibleVel  =  maxvel*f easibleVel/dmag; 

7,  end 
7. 

7«vel  =f easibleVel . *tstep; 


vel  =  zeros ( [2,4] ) ; 
vel  =  int8(vel); 

speedfile  =  f  open( ’N:  Wmatlabf  ile .  txt  ’ ,  ’w’); 

fprintf  (speedf ile ,  ,7od\n7od’,  vel (1 , 1)  , vel (2 , 1) ) ;  7«this  is  for  c  code 
f close(speedf ile) ; 

writedummy  =  f  open(  }N :  Wdummy .  txt ,  ,  ’w’); 
f close (writedummy) ; 

speedfile  =  f open( ’0 : Wmatlabf ile . txt ’ ,  ’w’); 

fprintf  (speedf  ile ,  ,7od\n7od’,  vel  (1 , 2)  , vel (2 , 2) ) ;  7othis  is  for  c  code 
f close(speedf ile) ; 

writedummy  =  f  open(  *  0 :  Wdummy .  txt  ’  ,  ’w’); 
f close (writedummy) ; 
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speedfile  =  f open (’P: \\matlabfile.txt’ ,  ’w’); 

fprintf  (speedf ile ,  } #/,d\n#/#d ’ ,  vel(l,3)  ,vel(2,3)) ;  °/0this  is  for  c  code 
f close(speedf ile) ; 

writedummy  =  f open( ’  P : \\ dummy . txt J ,  ’wJ); 
f close (writ edummy) ; 

7,  speedfile  =  f  open(  JR:  Wmatlabf  ile .  txt  * ,  JwJ); 

7*  fprintf  (speedf  ile ,  ,7od\n7»dJ,  vel(l,4)  ,vel(2,4)) ;  %this  is  for  c  code 
7.  f close(speedf ile) ; 

7. 

7.  writedummy  =  f  open(  ’R:  Wdummy .  txt  ’  ,  ’w’); 

7,  f  close  (writedummy)  ; 


function  [vel]  =  jacob(dx,dy,  theta) 

R  =  8.5;  7»in  cm 

W  =  32;  7»in  cm  taken  from  appendix  as  the  width 

D  =  13.5;7o  in  cm,  as  the  distance  from  the  middle  of  the  wheel  to  the 
7,  control  point, 

70note  that  this  is  inaccurate  because  the  control  point  will  vary  as  the 
7oimage  processor  calculates  the  centroid  of  the  small  dot. 

J  =  [  R/2*cos (theta) -R*D/W*  sin(theta) ,  R/2*cos (theta) +R*D/W*sin (theta) ; 

R/2*sin(theta)+R*D/W*  cos(theta),  R/2*sin(theta) -R*D/W*cos (theta)] ; 


vel  =  J'‘(-l)*  [dx;dy]  ; 
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K  Technical  specifications  for  replacement  batteries 


Q  VARTA 

Data  Sheet 


VH  4000  4/3 A 


Rechargeable  Ni-MH  Cylindrical 


c  b 


Type  Number: 

55140 

System: 

Nickel  Metal  Hydride/ 
KOH  Electrolyte 

Nominal  Voltage  [V]: 

1.2 

Nominal  Capacity  C  [mAh]: 

3600 

Typical  Capacity  C  [mAh]: 

at  800mA  / 1 ,00V 

4000 

Weight,  approx,  [g] 

55.0 

Dimensions  [mm]: 

min. 

max. 

Diameter  [a]: 

16.0 

17.0 

Height  [b]: 

65.5 

67.5 

Shoulder  Height  [c] 

65.2 

67.7 

Cap  diameter  [d] 

7.5 

8.5 

Temperature  Ranges  [°C] 

min. 

max. 

Storage:  less  than  30  days 

-20 

50 

less  than  90  days 

-20 

40 

less  than  1  year 

-20 

30 

Discharge: 

-20 

60 

Charge: 

0 

45 

Charging  Method: 

Normal  Charging: 
Accelerated  Charging  (20 °C): 


370mA  for  14-1 6h 


1100mA  for  4.75h 

Time  controlled,  voltage  control  recommended 
Fast  Charging:  (20°C)  3000mA  * 

Trickle  Charging:  pulsed  recommended 

Charge  Retention  [%]  at  20 °C:  min.  60% 

Capacity  available  after  1  month  Storage  at  20 °C  (cell  was  fast  charged) 


Impedance  [mOhm]:  max.  60 

at  charged  cells  (5  cycles),  20 °C,  AC:  1kHz,  (IEC  61951-2) 

Typical  Capacities  [mAh]: 

at  800mA  / 1 ,00V  4000 

at  2A  / 1 .00V  3700 

at  4A  / 1 .00V  3600 

Max.  Discharge  Current  (cont.)  [mA]:  5000 

Life  Expectancy  (typical): 

IEC  Cycle:  >500  Cycles 


*  (dT/  dt,  -dV) 

Capacities  based  on  normal  charging 


VARTA  Microbattery  GmbH,  DaimlerstraBe  1,  D-73479  Ellwangen/Jagst 
Tel.:  (+49)  7961/921-0,  Telefax:  (+49)  7961/921-553 


Subject  to  change  without  prior  notice  ! 

Date  of  Issue:  04-08-03 
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L  Programs  for  Simulations 

°/0workspace  creater 
y«22Jan 

clear 

clc 

global  stats 

robonum  =  5;  °/0the  number  of  robots  in  the  simulation 

figure (1) 
elf ; 

hold  on; 

axis (  [0  300  0  300]) 

for  k=  [1:1:3]  “/*  get  vertices  of  obstacles 

xys=ginput ; 
vx(k)  =  xys (1 , 1) ; 
vy  (k)  =  xys  (1 , 2)  ; 
plot (vx(k) , vy(k) ,  ’g  *’); 

end 

f ill(vx, vy,  ’k’ ) 

sarea=0 ; 
sex  =0; 
scy  =0; 

n=  (length(vx) ) ; 
for  i  =1:1: (n-1) 

sarea  =  sarea  +  (vx(i)*vy(i+l)  -  vx(i+l) *vy (i) ) ; 

sex  =  sex  +  (vx(i)+vx(i+l) )*(vx(i)*vy(i+l)  -  vx(i+l) *vy (i) ) ; 

scy  =  scy  +  (vy (i)+vy (i+1) ) * (vx(i) *vy (i+1)  -  vx(i+l) *vy (i) ) ; 

end 

area  =  abs( .5*(sarea  +  vx(n) *vy(l) -vx(l) *vy (n) ) ) ; 

Cx  =  abs (1/ (6*area) *  (scx+  (vx(n)+vx(l))*(vx(n)*vy(l)  -  vx(l) *vy (n) ) ) ) ; 
Cy  =  abs (1/ (6*area) *  (scy+  (vy(n)+vy(l))*(vx(n)*vy(l)  -  vx(l) *vy (n) ) ) ) ; 

plot(Cx,Cy,  Jr  dJ); 


i  m 

°/0find  the  angles  from  the  centroid  to  the  vetexes.  for  each  obstacle 
for  g= [1 : 1 : length (vx)] 

angles (g)  =  atan2(vy(g)-Cy,vx(g)-Cx) ; 

end 


rara%%rarararararammmn 


°/«sort  the  angles 

anglesxy  =[angles’ ,vx’ ,vy}] ; 


anglesxy  =  sortrows (anglesxy)  ;  °/0puts  the  angles  in  order,  x  and  y  change 

l  corispondingly 

stats (1) . angles  =  anglesxy (:, 1) ; 


stats (l).vx  =  anglesxy (: ,2) ; 
stats (l).vy  =  anglesxy (: ,3) ; 


i  nn 


stats(l) . Area  =  area; 
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stats (1) . Centroid(l)  =  Cx; 
stats (1) . Centroid(2)  =  Cy; 


0/«set  up  the  robot  positions 

for  k=  [1 : 1 :robonum] 
xys=ginput ; 
xbots(k)  =  xys(l,l); 
ybots(k)  =  xys(l,2); 

plot (xbots (k)  ,ybots (k)  ,  5r  >0;  °/oget  the  robot  positions 

end 

for  k= [1 : 1 : robonum] 
xys=ginput ; 
gx(k)  =  xys (1 , 1) ; 
gy  (k)  =  xys  (1,2)  ; 

plot (gx(k)  ,gy(k) ,  ’b  *0;  °/0get  the  goal  positions 

end 


#/0clear 

robonum  =  15; 
minsep  =  10; 
maxcom  =  50; 


figure (1) 
elf ; 

hold  on; 

axis([0  500  0  500]) 

°/oW=2 


for  w  =  28:28 

robonum  =  15; 
minsep  =  10; 
maxcom  =  50; 

%  figure (1) 

1  elf ; 

%  hold  on; 

1  axis( [0  500  0  500]) 


y.yoyoyomyoy.y0yoy.yoyoyoyoyoy.yoyoo/0°/.o/.o/oo/o0^ :y.yoy.yoyoyoy.yoy.yoyor/oo/oo/oo/oo/om°/o 


°/0set  up  goal  space 
flag  =true; 
count  =2; 

while (flag  &  count>=2) 

gxy  =  rand (2, robonum) *100+400; 
for  r  =1: robonum 
count  =0; 
for  i  =  1: robonum 


dist  =  sqrt ( (gxy (1 , i)  -  gxy(l,r))~2  +(gxy(2,i) 
gxy(2,r))~2); 


if  minsep  >  dist 
flag  =false; 
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break 

end 


if  dist  <=  maxcom 
count  =  count+1; 

end 

end 

end 

end 


plot  (gxy (1 ,  :  )  ,gxy (2 ,  :  )  ,  ’*>) 
gx  =  gxy ( 1 , : )  ; 
gy  =  gxy (2, :) ; 


y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.y.yoy.y.y.y.y.y.y.y,y*y.y.y*y,y.y*y,y.y.y.y.y.y.yx/.yoyx/.yo#/o#/o#/oyo 


°/0set  up  robot  positions 
xbots  =  rand(l ,robonum) *200; 
ybots  =  rand(l ,robonum) *200; 
plot (xbots ,ybots ,  3 r  .’) 


°/0set  up  triangle  obstacle 

°/0for  k=[l:l:3]  #/«  get  vertices  of  obstacles 


xys=g input ; 
vx  =  xys ( : , 1) ; 
vy  =  xys( : ,2) ; 
plot(vx,vy,  Jg  *’); 
Zend 

fill(vx,vy,  JkO 

sarea=0 ; 
sex  =0; 
scy  =0; 


n=  (length (vx)); 


for  i  =1:1: (n-1) 

sarea  =  sarea  +  (vx(i) *vy (i+1)  -  vx(i+l) *vy (i) ) ; 

sex  =  sex  +  (vx(i)+vx(i+l) ) * (vx(i) *vy(i+l)  -  vx(i+l) *vy (i) ) ; 

scy  =  scy  +  (vy (i)+vy (i+1) ) * (vx(i) *vy(i+l)  -  vx(i+l) *vy (i) ) ; 

end 


area  =  abs ( . 5* (sarea  +  vx(n)*vy(l)-vx(l)*vy(n))) ; 

Cx  =  abs (l/(6*area) *  (scx+  (vx(n)+vx(l))*(vx(n)*vy(l) 
vx(l) *vy (n) ) ) ) ; 


Cy  =  abs (l/(6*area) * 
vx(l) *vy (n) ) ) ) ; 


(scy+  (vy(n)+vy(l))*(vx(n)*vy(l) 


plot(Cx,Cy,  Jr  d’); 


°/o 


inn 

#/,find  the  angles  from  the  centroid  to  the  vetexes.  for  each  obstacle 
for  g= [1 : 1 : length (vx)] 

angles (g)  =  atan2(vy(g)-Cy ,vx(g)-Cx) ; 
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end 


mmmmmmmmmmmm% 


°/0sort  the  angles 
anglesxy  = [angles ’ ,vx,vy] ; 


anglesxy  =  sortrows (anglesxy)  ;  °/0puts  the  angles  in  order,  x  and  y 
7#  change  corispondingly 

stats (1) . angles  =  anglesxy (:, 1) ; 


stats (l).vx  =  anglesxy (: ,2) ; 
stats (l).vy  =  anglesxy (: ,3) ; 
stats(l) . Area  =  area; 
stats (1) . Centroid(l)  =  Cx; 
stats (1) . Centroid(2)  =  Cy; 


7.  mmmmmmmmmmmmmmmmmmm%mmnmm 
7.  mm 


7.  mmmmm%mmmmmmmmmmmmm%mmmmm% 

7.  7»7o7o7o7o7o 


7»set  up  square  obstacle 

7ofor  k=  [1:1:4]  7«  get  vertices  of  obstacles 

clear  xys,  vx,vy 

xys=ginput ; 

vx  =  xys ( : , 1) ; 

vy  =  xys( : ,2) ; 

plot(vx,vy,  Jg  **); 

7*end 


fill(vx,vy,  JkO 


sarea=0 ; 
sex  =0; 
scy  =0; 


n=  (length (vx) ) ; 


for  i  =1:1: (n-1) 

sarea  =  sarea  +  (vx(i) *vy (i+1)  -  vx(i+l) *vy (i) ) ; 

sex  =  sex  +  (vx(i)+vx(i+l) ) * (vx(i) *vy(i+l)  -  vx(i+l) *vy (i) ) ; 

scy  =  scy  +  (vy (i)+vy (i+1) ) * (vx(i) *vy(i+l)  -  vx(i+l) *vy (i) ) ; 

end 


area  =  abs ( . 5* (sarea  +  vx(n)*vy(l)-vx(l)*vy(n))) ; 

Cx  =  abs (l/(6*area) *  (scx+  (vx(n)+vx(l) ) *(vx(n)*vy(l) 
vx(l)*vy(n)))) ; 


Cy  =  abs (l/(6*area) * 
vx(l) *vy (n) ) ) ) ; 


(scy+  (vy(n)+vy(l))*(vx(n)*vy(l) 


plot(Cx,Cy,  5r  d’); 


7. 

7. 


mmmmmmmmmmmmmmmmmm%mmmmmn 

van 


7ofind  the  angles  from  the  centroid  to  the  vetexes.  for  each  obstacle 
for  g=[l: 1 :length(vx)] 

angles (g)  =  atan2(vy(g)-Cy , vx(g)-Cx) ; 

end 


m%mmmmmmmmmmm 
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7*sort  the  angles 
anglesxy  = [angles’ ,vx,vy] ; 

anglesxy  =  sortrows (anglesxy) ;  °/0puts  the  angles  in  order,  x  and  y 
7,  change  corispondingly 

stats (2) . angles  =  anglesxy (:, 1) ; 

stats (2). vx  =  anglesxy (: ,2) ; 
stats(2).vy  =  anglesxy (: ,3) ; 
stats (2) . Area  =  area; 
stats (2) . Centroid(l)  =  Cx; 
stats (2) . Centroid(2)  =  Cy; 


7.  mmraram%mmmmraramm%mmmmmmmramra% 

7.  7.7.7.7.7. 


filena  =  [’workspace 5 ,num2str(w)] ; 
save(filena, ’xbots’ , ’ybots’ , ’stats’ , ’gx’ , ’gy’) 
7»clear 

end 


function  [connectedness,  neargoal,  positions]  =. . . 
fullsim(xbots , ybots , stats ,gx,gy) ; 

7oTom  Dunbar 
7.24MAR 

70run  workspace  creator 


7.Parameters7,7o7o7o7o7o7o7o7o7o7.7o7o7o7o7o7o7o7o7o7o7o7o7o 

robonum  =15; 

tfin  =  500;  7#the  final  time 
tstep  =  .5;  7»the  time  incriment 

gscale  =20;  7»goal  scaler,  note  that  l/gscale*gdx 

oscale  =25;  7oObstacle  avoidence  scaler,  oscale*oadx 

Lscale  =  50;  7«L0S  scale 

maxvel  =  5;  7omaximum  velocity 

minsep  =10;  7ominimum  seperation  between  bots 

goal  cut  =  20/gscale;  7»maximum  distance  until  we  say  the  robot  is  near 

7,  enough  to  the  goal 

7o7»7o7.7o7o7.7o7o7o7o7o7*7o7o7.707.7«7o7.7o7o7o7o7o7o7o7«7*7o7o7o7o7. 


t  =0; 

cyclenum  =1; 

7»Pre  allocate  matrixes  to  imporove  computational  time 
numberof cycles  =  length( [0: tstep : tfin] ) ; 
connectedness  =  false ( [robonum, robonum, numberof cycles] ) ; 
positions  =  zeros ( [2 , robonum, numberof cycles] ) ; 
neargoal  =  numberof cycles*ones ( [1  robonum]); 

7o7.7o7o 


positions (1 ,:, cyclenum)  =  xbots;  7»these  come  from  the  workspacecreator 
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positions (2, :, cyclenum)  =  ybots; 

connectedness ( : , : , 1)  =  connections (positions (:,:, 1) , stats) ; 
7.  upper  =  triu (connects) ; 

7,  lower  =  tril  (connects)  ; 

7*  chec  =  upper  &  lower’; 

7,  connectedness (:,:, cyclenum)  =  chec; 

while (t<tf in) 

for  r  =  l:robonum  7ofor  each  robot 
P=[]  ; 

C  =  zeros(4,robonum) ; 


7«Goal  Velocity7o7»7.7.7«7o7.7»7.7.7.7o7»7.7.7«7.7.7«7.7.7.7o7.7.7o7«7.7.7»7.7.7«7o7.7.7o7«7.7o7» 

gdx  =  l/gscale*(gx(r)-xbots(r)) ; 
gdy  =  1/gscale* (gy(r) -ybots (r) ) ; 
7o7.7o7o7o7o7«7o7o7o707o7.7o7o7.7o7.7«7o7o7*7o7o707o7o7o7o7.7o7o7o7o7o7o7o7o7o7o7.707o7o7o7«7o7o7o707o 


if  (~(gdx  |  |  gdy))  7«if  both  equal  zero,  the  robot  is  at  the  goal 

7#’ robot  at  goal’ 

P(:,l)  =  [. 0001 ;. 0001]  ;  7»this  stops  an  error  when  the  robot 
7,  is  *exactly*  at  the  goal  which  never  happens 
else 

P(:,l)  =  [gdx ; gdy]  ;  7«a  goal  will  always  exist  except  if  the 

7,  robots  reach  exactly  the  goal, 
end 

if  norm ( [gdx ; gdy] )  <  goalcut 

if  cyclenum  <  neargoal  (r)  7«this  returns  a  vector  of  when  the 
7.  robot  reaches  near  its  goal,  if  it  never  gets  near  it, 
neargoal (r)  =  cyclenum;  7»the  vector  value  is  the  total 
7.  number  of  cycles . 

end 

end 


for  o  =  1 :  length  (stats)  7»for  each  obstacle 


^Obstacle  avoidance7o7o7o7«7o7«7o7o7o7«7o7o7o7o7o7o7o7o7o7o7»7o7o7o7o7.7.7o7o7.7o7« 

[oadx  oady]  =  Bishopobstacle(xbots(r) ,  ybots(r), 

7.7o7o7.7o7o787o7»7o7o7.707o7»7o7o707o7o707o7.7.7o7»707o7o7o7o7o7o7o707o7.7o7o7.7o7o7.7o7o7.7o7o7o7o7. 


o ,  stats) ; 


if  (sqrt  (oadx~2+oady'‘2) 
7.  to  the  controller 

P( : , end+1) 

end 


~=  0)  7«if  there  is  a  vector,  pass  it 
=  [oadx ; oady] . *oscale; 


7.L0S  stuff 7o7o7.7.7o7o7.707o7*7«7o7o7.7.7o7.7.7o7o7.7.7o7.7.7o7o7.7.7o7.7o7o7o7.7.7o7o7.7o7o 
for  q  =l:robonum  7«check  LOS  with  each  robot 
if  q  "=  r  7«the  robot  will  always  have  LOS  with  itself 
A  =  [stats(l) .vx’ ;stats(l) .vy’] ; 


xB  =  xbots(q); 
yB  =  ybots(q); 


Ab ( 1 , : )  =  A(1 , : )  -  xB; 

7.  B  is  at  the  origin,  do  it 
Ab(2, : ) 


7»translating  the  obstacle  so 
can  be  checked  by  LOSchecker 
=  A(2 , : )  -  yB; 


xtrans  =  xbots(r)  -xB; 
ytrans  =  ybots (r)  -yB; 
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[outside,  sanctum,  gammab,  gammat]  =. . . 

LOSconnect  (xtrans  ,ytrans , Ab)  ;  °/0is  LOS  needed  for  these  robots? 

if  ("sanctum)  7,if  it  is  NOT  in  the  inner  sanctum  or 
7,  outside  the  occlusion  lines 
7*then  LOS  needs  to  be  evaluated. 

connxr  =  sum (connectedness (r, :, cyclenum) )  -1; 
connxq  =  sum (connectedness (q, :, cyclenum) )  -1; 

7*  yominus  one  since  it  will  always  have  conx  with  itself 
if  connxr  <  connxq  7,if  r  has  less  connextions  than 
7.  q  r  needs  to  move 

[L0Sdx,L0Sdy]  =  LOSvelf (xtrans ,ytrans ,  gammab,... 
gammat,  outside); 

7oP(:,end+l)  =  [LOSdx;  LOSdy]  . *Lscale ; 

7»quiver (xbots(r)  ,ybots(r)  , LOSdx, LOSdy ,  ’m’) 

if  norm ([LOSdx,  LOSdy])  >  norm([C(l,q) ,C(2,q)] ) 

C(l,q)  =  L0Sdx*Lscale ;  7«if  this  is  a 
7,  larger  LOS  vector  use  it 
C(2,q)  =  L0Sdy*Lscale ; 

end 

end 

7»other  case 

if  connxr  ==  connxq  %  or  if  they  have  equal 

7.  connections,  the  lower  numbered  robot  needs  to  move 
if  r  <  q 

[LOSdx, LOSdy]  =  LOSvelf (xtrans ,ytrans ,.. . 
gammab,  gammat,  outside); 

7oP(:,end+l)  =  [LOSdx;  LOSdy] . *Lscale ; 


7.  7«quiver  (xbots(r)  ,ybots(r)  ,L0Sdx*Lscale , LOSdy , 
if  norm ([LOSdx,  LOSdy])  >  norm( [C(l ,q) ,C(2,q)] ) 
C(l,q)  =  L0Sdx*Lscale ;  7« if  this  is  a 

7.  larger  LOS  vector  use  it 
C(2,q)  =  LOSdy*Lscale ; 

end 


end 

end 

end 

end  7»end  if  q~=r  statement 

7o7.7o7.7.7o7«7«7o7.7.7o7o7o7o7«7o7o7o7o7.7«7o7o7o7o7«7.7o7»7«7o7«7o7o7»7o7«7o7o7»7«7. 

end  7»end  for  each  robot  q 
end  7»end  for  each  obstacle 


’m’) 


7oRange  part7.7o7o7.m7.7.7o7o7o7o7o7.m7o7.7.7o7.7o7. 

for  q  =  l:robonum  7»for  each  robot 
if  r  ~=q 

dist  =  sqrt((xbots(q)-xbots(r))~2  +  (ybots (q) -ybots (r) ) . 
"2)  ;  7odist  =  0  if  they  are  the  same 


xunit  =  (xbots(q)  -  xbots  (r)  ) /dist ;  70must  be  written 
7.  this  way  to  ensure  vector  is  pointing  the  correct  way. 

yunit  =  (ybots (q)  -  ybots (r) ) /dist ; 


if  (dist  <  minsep)  /odist  must  be  nonzero 

dist  =  dist/minsep;  7«on  a  scale  from  0  to  1  how  small 
7*  is  the  distance  compared  to  minsep 
delta  =.01; 
h  =.556929; 
xsig  =1 . 1229*h; 
c  =  7.05538; 


yomexhat 
zplus  =. . . 

(l/(2*pi*xsig) )*c* (h- (dist+delta) ~2/xsig~2) *exp(-l* (dist+delta)  ~2/(2*xsig 

2)); 


zminus  =. . . 

(l/(2*pi*xsig) )*c*  (h-  (dist-delta)  ~2/xsig~2)  *exp(-l*  (dist-delta)  ~2/(2*xsig 

2)); 


dz  =  (zplus-zminus)/(2*delta) ; 
P(:,end+1)  =  [xunit;  yunit].*dz;  °/«if  the  distance  is 
°/0  less  than  minimum  seperation, 
else 

dz  =  Improvedsemicont (dist) ; 

C(3,q)  =  dz*xunit ;  “/(velocity  times  unit  vector 
C(4,q)  =  dz*yunit; 

end 

end 

end 


yoyoyoyoy.yoyoyoyoyoyoyoyoyoyoy.y0yoy.yoyoy.yoyoy0yoy.yoyoyoy :yoy.yoyoyoyoyoy0y.yor/oy.#/oo/oo/.o/oo/oo/0o/.o/.o/0°/o#/. 


•/,  if  r  ==1 

°/0  [M  N]  =  size(P); 

#/«  for  u  =  1:N 

#/«  quiver (xbots (r)  ,ybots(r)  ,P(1  ,u)  ,P(2,u)  ,  ’k’ ) 
°/0  end 


°/«  [M  N]  =  size(C); 

#/«  for  c=l:N 

l  quiver (xbots (r) ,ybots(r) , C(1 , c) ,C(2 , c) , ’g’ ) 

#/«  quiver  (xbots  (r)  ,ybots(r)  ,C(3,  c)  ,C(4,  c)  ,  ’r  ’  ) 

#/«  end 

#/,  end 


#/.  P 
°/.  C 

commnum  =  sum(connectedness  (r ,  :  ,  cyclenum)  ) -1 ;  °/0minus  one  since  it 
#/0  will  have  corns  with  itself 

commsindx  =  f ind(connectedness (r , : , cyclenum)) ; 


A  =  paracontrollerupdate(P,C,  commsindx,  commnum); 


N  =  size(A) ; 
if  N (2) ==1 ; 

feasibleVel  =A; 

else 

°/.A 

feasibleVel  =  smartSum(A) ; 

end 


dmag  =  norm(f easibleVel) ; 
if  (dmag  >maxvel) 

feasibleVel  =  maxvel*f easibleVel/dmag; 

end 


dvect ( : , r)  =f easibleVel . *tstep ; 


end 

xbots  =  xbots+dvect (1 , : ) ; 
ybots  =  ybots+dvect (2, : ) ; 


cyclenum  =  cyclenum+1 ; 

posit ions ( 1, cyclenum)  =  xbots; 
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positions (2 cyclenum)  =  ybots; 

connectedness ( : , : , cyclenum)  = . . . 
connections (positions ( : , : .cyclenum) , stats) ; 

t  =t+tstep; 

end 


function  [connectedness,  neargoal,  positions]  =... 
fullsimcomms (xbots , ybots , stats ,gx,gy) ; 

"/.Tom  Dunbar 
7.24MAR 

'/.run  workspace  creator 


XPararnetersmmmmmnram 
robonum  =15; 

tfin  =  500;  °/0the  final  time 
tstep  =  .5;  °/0the  time  incriment 

gscale  =20;  °/0goal  scaler,  note  that  l/gscale*gdx 

oscale  =25;  “/obstacle  avoidence  scaler,  oscale*oadx 

Lscale  =  50;  °/0LOS  scale 

maxvel  =  5;  “/maximum  velocity 

minsep  =10;  "/minimum  seperation  between  bots 

goal  cut  =  20/gscale;  "/maximum  distance  until  we  say  the  robot  is  near 
7#  enough  to  the  goal 

mmm/mmmmrararann 


t  =0; 

cyclenum  =1; 

"/Pre  allocate  matrixes  to  imporove  computational  time 
numberof cycles  =  length( [0: tstep : tfin] ) ; 
connectedness  =  false ( [robonum, robonum, numberof cycles] ) ; 
positions  =  zeros ( [2, robonum, numberof cycles] ) ; 
neargoal  =  numberof cycles*ones ( [1  robonum]); 

7.7.77 


positions (1 ,:, cyclenum)  =  xbots;  "/these  come  from  the  workspacecreator 
positions (2, : .cyclenum)  =  ybots; 

connectedness ( : , : , 1)  =  connections (positions 1) , stats) ; 

7.  upper  =  triu(connects)  ; 

7.  lower  =  tril  (connects)  ; 

7.  chec  =  upper  &  lower’; 

7.  connectedness (:,:, cyclenum)  =  chec; 

while (t<tf in) 

for  r  =  1:  robonum  "/.for  each  robot 
P=[]  ; 

C  =  zeros (4, robonum) ; 
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•/.Goal  velocit yV.’/X/X/Xm^^^ 

gdx  =  l/gscale*(gx(r)-xbots(r)) ; 
gdy  =  l/gscale*(gy(r)-ybots(r)) ; 

%m%mraram%mmramrararam%mnra 


if  norm ( [gdx ; gdy] )  <  goalcut 

if  cyclenum  <  neargoal  (r)  #/0this  returns  a  vector  of  when  the 
7.  robot  reaches  near  its  goal,  if  it  never  gets  near  it, 
neargoal (r)  =  cyclenum;  7,the  vector  value  is  the  total 
%  number  of  cycles . 

end 

end 


for  o  =  1 : length(stats)  7»for  each  obstacle 


^obstacle  avoidancem%mmmmm%mmmra 

[oadx  oady]  =  Bishopobstacle(xbots(r) ,  ybots(r), 

m%mramm%mmmmramram%%mran 


o ,  stats) ; 


if  (sqrt  (oadx~2+oady'‘2) 
7*  to  the  controller 

P( : , end+1) 

end 


~=  0)  7»if  there  is  a  vector,  pass  it 
=  [oadx ; oady] .*oscale; 


7.L0S  stuff 7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7o7»7o7o7o7o7o7o7o7o7o7o7o7o7o 
for  q  =l:robonum  7, check  LOS  with  each  robot 
if  q  ’=  r  7«the  robot  will  always  have  LOS  with  itself... 
A  =  [stats(l) .vx5 ;stats(l) .vy’] ; 

xB  =  xbots(q); 
yB  =  ybots(q); 


Ab(l,:)  =  A ( 1 ,  : )  -  xB;  7»translating  the  obstacle  so 
7,  B  is  at  the  origin,  do  it  can  be  checked  by  LOSchecker 
Ab(2, : )  =  A(2 , : )  -  yB; 


xtrans  =  xbots(r)  -xB; 
ytrans  =  ybots(r)  -yB; 


[outside,  sanctum,  gammab,  gammat]  =. . . 

LOSconnect  (xtrans , ytrans  ,Ab)  ;  7«is  LOS  needed  for  these  robots? 
if  ("sanctum)  7«if  it  is  NOT  in  the  inner  sanctum  or 
7,  outside  the  occlusion  lines 
7othen  LOS  needs  to  be  evaluated. 

connxr  =  sum (connectedness (r, :, cyclenum) )  -1; 
connxq  =  sum (connectedness (q, :, cyclenum) )  -1; 

7,  Tminus  one  since  it  will  always  have  conx  with  itself 
if  connxr  <  connxq  7, if  r  has  less  connextions  than 

l  q  r  needs  to  move 

[L0Sdx,L0Sdy]  =  LOSvelf (xtrans , ytrans ,  gammab,... 

gammat,  outside); 


7»P(:,  end+1)  =  [LOSdx;  LOSdy]  .  *Lscale ; 

7«quiver (xbots(r)  ,ybots(r)  , LOSdx, LOSdy ,  ’m’) 
if  norm ([LOSdx,  LOSdy])  >  norm( [C(l ,q) ,C(2 ,q)] ) ; 
C(l,q)  =  L0Sdx*Lscale ;  7«if  this  is  a 

7.  larger  LOS  vector  use  it 

C(2,q)  =  L0Sdy*Lscale ; 

end 


end 


#/*other  case 

if  connxr  ==  connxq  °/0  or  if  they  have  equal 

7.  connections,  the  lower  numbered  robot  needs  to  move 
if  r  <  q 

[LOSdx, LOSdy]  =  LOSvelf (xtrans ,ytrans , . . . 

gammab,  gammat,  outside); 

7.P(:,end+l)  =  [LOSdx;  LOSdy]  .  *Lscale ; 


7.  °/0quiver (xbots(r)  ,ybots(r)  ,LOSdx*Lscale , LOSdy,  ’mO 
if  norm ([LOSdx,  LOSdy])  >  norm( [C ( 1 ,q) ,C(2,q)] ) ; 

C(l,q)  =  LOSdx*Lscale ;  %if  this  is  a 

7,  larger  LOS  vector  use  it 

C(2,q)  =  LOSdy*Lscale ; 

end 

end 

end 

end 

end  °/0end  if  q~=r  statement 

end  °/0end  for  each  robot  q 
end  °/0end  for  each  obstacle 


•/.Range  partmmmrammmmmmmmmn 

for  q  =  l:robonum  Zfor  each  robot 
if  r  ~=q 

dist  =  sqrt((xbots(q)-xbots(r))~2  + 
(ybots(q)-ybots(r))~2) ;  °/0dist  =  0  if  they  are  the  same 


xunit  =  (xbots(q)  -  xbots  (r)  ) /dist ;  ’/.must  be  written 
7#  this  way  to  ensure  vector  is  pointing  the  correct  way. 

yunit  =  (ybots(q)  -  ybots (r) ) /dist ; 


if  (dist  <  minsep)  7.dist  must  be  nonzero 

dist  =  dist/minsep;  7.on  a  scale  from  0  to  1  how  small 
7.  is  the  distance  compared  to  minsep 
delta  =.01; 
h  =.556929; 
xsig  =1.1229*h; 
c  =  7.05538; 

7.mexhat 
zplus  =. . . 

(l/(2*pi*xsig) )*c* (h- (dist+delta) ~2/xsig~2) *exp(-l* (dist+delta) ~2/(2*xsig 

2)); 


zminus  = . . . 

(l/(2*pi*xsig) )*c* (h- (dist-delta) ~2/xsig~2) *exp(-l* (dist-delta) ~2/(2*xsig 

2)); 


dz  =  (zplus-zminus)/(2*delta) ; 
P(:,end+1)  =  [xunit;  yunit]. *dz;  °/.if  the  distance  is 
7.  less  than  minimum  seperation, 
else 

dz  =  Improvedsemicont (dist) ; 

C(3,q)  =  dz*xunit;  ^velocity  times  unit  vector 
C(4,q)  =  dz*yunit; 

end 

end 

end 


7.7o7.7.7o7o7.7o7o7.7.7o7o7o7o7o7o7.7o7o7.7o7o7.7o7o7o7.7.7o7o7.7o7o7o7o7o7.7o7o7o7o7.7o7o7.7o7o7.7o7.7.7o7o0/o 


7.  if  r  ==l 
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7.  [M  N]  =  size(P); 

7*  for  u  =  1:N 

7.  quiver (xbots (r)  ,ybots(r)  ,P(1 ,u) ,P(2,u) , ’k’ ) 

7.  end 

7.  [M  N]  =  size(C); 

7.  for  c=l:N 

7,  quiver  (xbots  (r)  ,ybots(r)  ,C(1 ,  c)  ,C(2 ,  c)  ,  ’g’  ) 

7.  quiver  (xbots  (r)  ,ybots  (r)  ,C(3,  c)  ,C(4,  c)  ,  ’r  ’  ) 

7,  end 

7,  end 
7.  P 
7.  c 

commnum  =  sum  (connectedness  (r cyclenum)  ) -1 ;  7»minus  one  since  it 
7,  will  have  corns  with  itself 

commsindx  =  f ind(connectedness (r , : , cyclenum)) ; 

A  =  paracontrollerupdate(P,C,  commsindx,  commnum); 

feasible  =  f easiblity ( [A, [gdx;gdy] ] ) ; 
if  (feasible)  7«this  is  what  seperates  this  from  the  regular  full 

7,  sim 

A(:,end+1)  =  [gdx;gdy]  ;  7«only  after  we  have  a  feasible  comm 
7,  vector,  is  it  written  to  A 
end 


N  =  size(A) ; 
if  N (2) ==1 ; 

feasibleVel  =A; 

else 

7.A 

feasibleVel  =  smartSum(A) ; 

end 

dmag  =  norm(f easibleVel) ; 
if  (dmag  >maxvel) 

feasibleVel  =  maxvel*f easibleVel/dmag; 

end 

dvect(:,r)  =f easibleVel . *tstep; 


end 

xbots  =  xbots+dvect (1 , : ) ; 
ybots  =  ybots+dvect (2, : ) ; 

cyclenum  =  cyclenum+1; 

posit ions ( 1, :, cyclenum)  =  xbots; 

positions (2 ,:, cyclenum)  =  ybots; 

connectedness ( : , : , cyclenum)  = . . . 
connections (positions ( : , : , cyclenum) , stats) ; 

t  =t+tstep; 


end 
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°/0f unction  [connectedness,  neargoal,  positions]  = 
°/0f ullsimrestric  (xbots , ybots ,  stats , gx , gy)  ; 

#/oTom  Dunbar 
7o24MAR 

°/0run  workspace  creator 


•/.Pararnetersmm%%%%%mm%%m%% 

robonum  =3; 

tfin  =  50;  °/0the  final  time 
tstep  =  .5;  “/the  time  incriment 

gscale  =20;  °/0goal  scaler,  note  that  l/gscale*gdx 

oscale  =25;  “/obstacle  avoidence  scaler,  oscale*oadx 

Lscale  =  50;  °/0LOS  scale 

maxvel  =  5;  "/maximum  velocity 

minsep  =10;  "/minimum  seperation  between  bots 

goal  cut  =  20/gscale;  "/maximum  distance  until  we  say  the 

"/robot  is  near  enough  to  the  goal 


raramrammmmmramn 


t  =0; 

cyclenum  =1; 

"/Pre  allocate  matrixes  to  imporove  computational  time 
numberof cycles  =  length( [0: tstep : tfin] ) ; 
connectedness  =  false ( [robonum, robonum, numberof cycles] ) ; 
positions  =  zeros ( [2, robonum, numberof cycles] ) ; 
neargoal  =  numberof cycles*ones ( [1  robonum]); 

#/m 


positions (1 ,:, cyclenum)  =  xbots;  "/these  come  from  the  workspacecreator 
positions (2, :, cyclenum)  =  ybots; 

connectedness ( : , : , 1)  =  connections (positions 1) , stats) ; 

groups  =  grouper (connectedness ( : , : , cyclenum)) ; 

*/  upper  =  triu (connects)  ; 

*/  lower  =  tril  (connects)  ; 

*/  chec  =  upper  &  lower’; 

*/  connectedness (:,:, cyclenum)  =  chec; 

while (t<tf in) 

for  r  =  1:  robonum  "/for  each  robot 
commsindx  =  []  ; 
clear  xbots; 
clear  ybots; 

groupnum  =  f  ind(groups  (  :  ,r)  )  ;  "/find  which  group  the  robot  belongs 
*/0to 

groupidx  =  f ind (groups (groupnum,  :))  ;  "/find  which  robots  are  in  that 

"/group 

counter  =0; 

for  i  =  groupidx; 

counter  =  count er+1; 

xbots  (counter)  =  positions  (1 ,  i ,  cyclenum)  ;  "/make  xbots  out  of 
“/that  group. 

ybots (counter)  =  positions (2 , i , cyclenum) ; 
if  connectedness (r,i, cyclenum) 
commsindx (end+1)  =  counter; 

end 

end 

x  =  posit ions (l,r, cyclenum) ; 
y  =  positions(2,r, cyclenum) ; 
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p=[] ; 

C  =  zeros(4,length(xbots)) ; 


8/.Goai  veiocityxmmmmmmmmmmmmmji 
gdx  =  l/gscale*(gx(r)-x) ; 
gdy  =  1/gscale* (gy (r)-y) ; 

%7//.7o%y//o7.yoyo%%%0/.7oyo%%7.0///o7.0/„%7.7oyo0/o%%%7o7.0/o7.7.0///o7.%%0/.7o7.0/o%7.°/.yo7.% 


if  (~(gdx  I  I  gdy))  '/.if  both  equal  zero,  the  robot  is  at  the  goal 
“/o’ robot  at  goal5 

P(:,l)  =  [. 0001 0001]  ;  '/.this  stops  an  error  when  the  robot 
#/«is  *exactly*  at  the  goal  which  never  happens 

else 

P(:,l)  =  [gdx ; gdy]  ;  "/.a  goal  will  always  exist  except  if  the 

°/0robots  reach  exactly  the  goal. 

end 

if  norm ( [gdx ; gdy] )  <  goalcut 

if  cyclenum  <  neargoal  (r)  °/0this  returns  a  vector  of  when  the 
°/0robot  reaches  near  its  goal,  if  it  never  gets  near  it, 
neargoal (r)  =  cyclenum;  °/0the  vector  value  is  the  total 
"/number  of  cycles . 

end 

end 


for  o  =  1 : length(stats)  "/for  each  obstacle 


"/.obstacle  avoidancemmmmmmmmmmn 

[oadx  oady]  =  Bishopobstacle(x,  y,  o,  stats); 

mmmrammmraramm%%mramrara% 


if  (sqrt  (oadx"'2+oady''2)  ~=  0) 

'/.it  to  the  controller 
P(:,end+1)  =  [oadx ; oady] . *oscale ; 

end 


"/.if  there  is  a  vector,  pass 


"/.los  stuffmmmmmmmmmmmmmn 

for  q  =1 : length(xbots)  "/.check  LOS  with  each  robot 

if  xbots(q)  ~=  x  "/.the  robot  will  always  have  LOS  with  itself 
A  =  [stats(l) .vx5 ;stats(l) .vy5]  ; 

xB  =  xbots(q); 
yB  =  ybots(q); 

Ab(l,:)  =  A(l,:)  -  xB;  '/.translating  the  obstacle  so  B 
'/.is  at  the  origin,  do  it  can  be  checked  by  LOSchecker 
Ab(2, : )  =  A(2 , : )  -  yB; 

xtrans  =  x  -xB; 
ytrans  =  y  -yB; 


[outside,  sanctum,  gammab,  gammat]  =  ... 

LOSconnect (xtrans , ytrans , Ab) ; 

'/.is  LOS  needed  for  these  robots? 
if  (~ sanctum)  '/.if  it  is  NOT  in  the  inner  sanctum 
'/.or  outside  the  occlusion  lines 
'/.then  LOS  needs  to  be  evaluated, 
connxr  =  sum (connectedness (r, :, cyclenum) )  -1; 
connxq  =sum(connectedness(groupidx(q) , : , cyclenum)) . . 
-1;  '/.minus  one  since  it  will  always  have  conx  with 


Ill 


'/.itself 

if  connxr  <  connxq  °/0if  r  has  less  connextions  than 
"/q  r  needs  to  move 
[L0Sdx,L0Sdy]  =  ... 

LOSvelf (xtrans ,ytrans ,  gammab,  gammat,  outside); 
70P(:,end+l)  =  [LOSdx;  LOSdy]  . *Lscale ; 
°/0quiver(xbots(r)  ,ybots(r)  , LOSdx, LOSdy ,  ’mJ) 
if  norm ([LOSdx,  LOSdy])  >  norm( [C ( 1 ,q) ,C(2,q)] ) 
C(l,q)  =  LOSdx*Lscale ;  "/.if  this  is  a 

“/larger  LOS  vector  use  it 
C(2,q)  =  LOSdy*Lscale ; 

end 

end 

"/other  case 

if  connxr  ==  connxq  “/  or  if  they  have  equal 
"/connections,  the  lower  numbered  robot  needs  to  move 
if  r  <  groupidx(q) 

[LOSdx, LOSdy]  =. . . 

LOSvelf (xtrans ,ytrans , . . . 
gammab,  gammat,  outside); 

°/.P(:,end+l)  =  [LOSdx;  LOSdy] . *Lscale ; 

"/quiver (xbots (r)  ,ybots(r)  ,LOSdx*Lscale, LOSdy,  ’m’  ) 
if  norm ([LOSdx,  LOSdy])  >... 

norm( [C(l ,q) ,C(2,q)] ) 

C(l,q)  =  LOSdx*Lscale ;  */0if  this  is  a 

"/larger  LOS  vector  use  it 
C(2,q)  =  LOSdy*Lscale ; 

end 

end 

end 


end 


end 


end  "/end  if  q~=r  statement 


mrararararararanra: ninininni 


end  "/end  for  each  robot  q 
"/end  for  each  obstacle 


"/.Range  part raramm%mramrammmm%mn 

for  q  =  1 : length(xbots)  “/.for  each  robot 
if  x  ~=  xbots (q) 

dist  =  sqrt  (  (xbots  (q) -x)  "2  +  (ybots  (q)  -y)  ~2)  ;  "/.dist 
"/.they  are  the  same 


0  if 


xunit  =  (xbots  (q)  -  x)/dist;  "/.must  be  written  this  way 
"/.to  ensure  vector  is  pointing  the  correct  way. 
yunit  =  (ybots (q)  -  y)/dist; 


if  (dist  <  minsep)  "/.dist  must  be  nonzero 

dist  =  dist/minsep;  "/.on  a  scale  from  0  to  1  how 
"/.small  is  the  distance  compared  to  minsep 
delta  =.01; 
h  =.556929; 
xsig  =1 . 1229*h; 
c  =  7.05538; 

"/.mexhat 

zplus  =  (1/ (2*pi*xsig) )  *c*  (h-  (dist+delta)  ~2/xsig~2)  .  .  . 

*exp(-l* (dist+delta) ~2/ (2*xsig~2) ) ; 
zminus  =  (1/ (2*pi*xsig) ) *c* (h- (dist-delta) ~2/xsig~2) . . . 

*exp(-l* (dist-delta) ~2/ (2*xsig~2) ) ; 
dz  =  (zplus-zminus)/(2*delta) ; 

P(:,end+1)  =  [xunit;  yunit]. *dz;  "/.if  the  distance 
"/.is  less  than  minimum  seperation, 

else 
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dz  =  Improvedsemicont (dist) ; 

C(3,q)  =  dz*xunit;  "/velocity  times  unit  vector 
C(4,q)  =  dz*yunit; 

end 

end 

end 


rarara%mmmmrararararararammmm 


if  r  ==1 

[M  N]  =  size(P) ; 
for  u  =  1:N 

quiver (xbots(r) ,ybots(r) ,P(1 ,u) ,P(2,u) , ’k’ ) 

end 

[M  N]  =  size(C) ; 
for  c=l:N 

quiver (xbots(r) ,ybots(r) ,C(l,c) ,C(2,c) , ’g’) 
quiver (xbots(r) ,ybots(r) ,C(3,c) ,C(4,c) , ’r’) 

end 

end 

P 

C 


commnum  =  sum  (connectedness  (r ,  :  ,  cyclenum)  ) -1 ;  "/minus  one  since 

"/it  will  have  corns  with  itself 

"/commsindx  =  find  (connectedness  (r ,  cyclenum)  )  ; 


A  =  paracontrollerupdate(P,C,  commsindx,  commnum); 


N  =  size(A) ; 
if  N (2) ==1 ; 

feasibleVel  =A; 

else 

°/A 

feasibleVel  =  smartSum(A) ; 

end 


dmag  =  norm (feasibleVel) ; 
if  (dmag  >maxvel) 

feasibleVel  =  maxvel*f easibleVel/dmag; 

end 

dvect ( : , r)  =f easibleVel . *tstep ; 


end 

*/  xbots  =  xbots+dvect  (1 ,  :  )  ; 

*/  ybots  =  ybots+dvect  (2 ,  :  )  ; 

7. 

cyclenum  =  cyclenum+1; 

posit ions ( 1, cyclenum)  =  positions (1 cyclenum-1)  +  dvect(l,:); 
positions (2 cyclenum)  =  positions (2 cyclenum-1)  +  dvect(2,:); 

connectedness ( : , : , cyclenum)  =  connections (positions ( : , : , cyclenum) , stats) ; 
groups  =  grouper (connectedness ( : , : , cyclenum)) ; 
t  =t+tstep; 


end 
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70simulation  plotter 

70removed  all  of  the  plotting  elements  from  the  simulation  for  comutational 
70time  purposes,  this  way  I  can  just  grab  a  figure  and  mess  with  it  from 
°/0there . 

figure (1) ; 
hold  on; 

[a  b  c]  =size(positions) ; 
for  n  =  l:b 

xpositions (1 , 1 : c)  =  positions (1 ,n, 1 : c) ; 
ypositions (1 , 1 : c)  =  positions (2 ,n, 1 : c) ; 

plot (xpositions ,ypositions ,  ’b’) ; 

end 
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M  Programs  for  baseline  simulations 

function  [connectedness,  neargoal]  =  baseline (xbot s, ybot s, stat s, gx, gy) ; 
#/0Tom  Dunbar 

°/0Baseline  function,  this  just  has  the  obstacle  avoidance,  goal  and 
'/interrobot  collision  smart  summed  together 

'/.run  workspace  creator 


XPararnetersmmmmmmmm 

robonum  =  15;  °/0the  number  of  robots  in  the  simulation 
tfin  =  500;  °/0the  final  time 
tstep  =  .5;  °/0the  time  incriment 

gscale  =20;  °/0goal  scaler,  note  that  l/gscale*gdx 
oscale  =25;  “/obstacle  avoidence  scaler,  oscale*oadx 
Lscale  =  50;  0/0LOS  scale 
maxvel  =  5;  “/maximum  velocity 
minsep  =  10; 

goal  cut  =  20/gscale;  “/maximum  distance  until  we  say  the  robot  is  near 
*/  enough  to  the  goal 

mra%%mrara%%ramraram% 


t  =0; 

cyclenum  =1; 

°/0Pre  allocate  matrixes  to  imporove  computational  time 
numberof cycles  =  length( [0: tstep : tfin] ) ; 
connectedness  =  false ( [robonum, robonum, numberof cycles] ) ; 
positions  =  zeros ( [2 , robonum, numberof cycles] ) ; 
neargoal  =  numberof cycles*ones ( [1  robonum]); 

7.7.7. 


positions (1 ,:, cyclenum)  =  xbots;  “/these  come  from  the  workspacecreator 
positions (2, :, cyclenum)  =  ybots; 

connectedness 1)  =  connections (positions 1) ,  stats); 


while (t<tf in) 

for  r  =  1:  robonum  "/for  each  robot 


P  =□; 

“/Goal  veiocityxmmmmmmmmmmmmxm 

gdx  =  1/gscale* (gx(r) -xbots (r) ) ; 
gdy  =  1/gscale* (gy(r) -ybots (r) ) ; 

ra7.mmmmmramra7.mmmmmm%ra 


if  (~(gdx  |  |  gdy))  '/if  both  equal  zero,  the  robot  is  at  the  goal 

'/’robot  at  goal’ 

P(:,l)  =  [. 0001 ;. 0001]  ;  '/this  stops  an  error  when  the  robot 
'/  is  *exactly*  at  the  goal  which  never  happens 
else 

P(:,l)  =  [gdx ; gdy]  ;  '/a  goal  will  always  exist  except  if  the 

'/  robots  reach  exactly  the  goal, 
end 

if  norm ( [gdx ; gdy] )  <  goalcut 

if  cyclenum  <  neargoal  (r)  '/this  returns  a  vector  of  when  the 
*/  robot  reaches  near  its  goal,  if  it  never  gets  near  it, 
neargoal (r)  =  cyclenum;  '/the  vector  value  is  the  total 
'/  number  of  cycles . 

end 
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end 


for  o  =  1 :  length  (stats)  °/,for  each  obstacle 


■/.obstacle  avoidancemmm%ram%%mm%%mm 

[oadx  oady]  =  Bishopobstacle(xbots(r) ,  ybots(r), 

mmmmmmramramraram%mranm 


o ,  stats) ; 


if  (sqrt  (oadx~2+oady'‘2)  ~=  0)  °/«if  there  is  a  vector,  pass  it 

"/,  to  the  controller 

P(:,end+1)  =  [oadx ; oady] . *oscale ; 

end 

end  "/.end  for  each  obstacle 


■/.Range  partmmmmmmmmmmmmmxm 

for  q  =  1 :  length (xbots)  °/.for  each  robot 
if  r  ~=q 

dist  =  sqrt ( (xbots (q) -xbots (r))~2  + 
(ybots(q)-ybots(r))~2) ;  "/.dist  =  0  if  they  are  the  same 


xunit  =  (xbots  (q)  -  xbots  (r)  )  /dist ;  "/.must  be  written 

■/.  this  way  to  ensure  vector  is  pointing  the  correct  way. 

yunit  =  (ybots(q)  -  ybots (r) ) /dist ; 


if  (dist  <  minsep)  "/.dist  must  be  nonzero 

dist  =  dist/minsep;  "/.on  a  scale  from  0  to  1  how  small 
■/.  is  the  distance  compared  to  minsep 
delta  =.01; 
h  =.556929; 
xsig  =1 . 1229*h; 
c  =  7.05538; 

"/.mexhat 
zplus  = . . . 

(l/(2*pi*xsig) )*c* (h- (dist+delta) ~2/xsig~2) *exp(-l* (dist+delta) ~2/(2*xsig'‘ . . . 

2)); 


zminus  =. . . 

(l/(2*pi*xsig) )*c* (h- (dist-delta) ~2/xsig~2) *exp(-l* (dist-delta) ~2/(2*xsig'' . . . 

2)); 


dz  =  (zplus-zminus)/(2*delta) ; 
P(l,end+1)  =  dz*xunit; 

P(2,end+1)  =  dz*yunit;  °/.if  the  distance  is  less  than 
*/.  minimum  seperation, 
end 

end 

end 


feasibleVel  =  paracontrollerbase(P) ; 

"/.figure  (1) 

"/.quiver (xbots (r)  ,ybots(r)  ,f easibleVel (1)  ,f easibleVel(2) ,  ’m’) 
dmag  =  norm(f easibleVel) ; 
if  (dmag  >maxvel) 

feasibleVel  =  maxvel*f easibleVel/dmag; 

end 

dvect(:,r)  =f easibleVel . *tstep; 


end 
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xbots  =  xbots+dvect (1 , : ) ; 
ybots  =  ybots+dvect (2, : ) ; 

cyclenum  =  cyclenum+1 ; 
positions (1 cyclenum)  =  xbots; 
positions (2 cyclenum)  =  ybots; 


connectedness ( : , : , cyclenum)  =  connections (positions ( :  ,  :  , cyclenum) , . . . 
stats)  ; 

t  =t+tstep; 

end 


function  feasibleVel  =  paracontrollerbase(P)  °/0P  is  a  2-N  priority  matrix 

/  which  is  in  the  format  of  rows:  dx;dy 

“/.C  is  a  4-N  comm  matrix  which  is  in 

°/0the  f  ormat ,  rows :  dxLOS ;  dyLOS ;  dxr ange ;  dyrange 

“/Tom  Dunbar 
'/Trident  Project 
“/.24MAR 


rarararammmmmmmrarararararammmnras 


"/Check  if  all  priority  1  velocities  are  feasible 


feasible  =  f easiblity (P) ; 

A=P ; 

while ("feasible)  #/«while  it  is  not  feasible 
D=  []  ; 

veloA  =  []  ;  “/blanks  it  out  or  else  an  infinite  loop  will  occur!!! 

S  =  size(A) ; 
for  i  =  1 : S (2) 

veloA(i)  =  norm(A( : , i) ) ; 

end 

[velo  i]  =  min(veloA); 

D(l,:)  =  [A ( 1 , 1 :  i-1)  ,  A(1 ,  i+1 : end)]  ;  “/delete  the  smallest  velocity 
D(2, : )  =  [A (2 , 1 : i-1) , A(2 , i+1 : end) ] ; 

feasible  =  f easiblity (D) ;  “/recheck  feasibility 
A  =D;  “/after  the  smallest  has  been  deleted  update  A; 

"/note  that  velocities  are  sucessively  deleted,  and  thus  feasibility 
"/will  eventually  be  acheived  because  soon  there  will  only  be  two 
“/velocities  left. 

end 

°/A 

S  =  size(A) ; 
if  S (2) ==1 ; 

feasibleVel  =A; 
return 
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end 

°/0Now  that  we  have  a  set  of  feasible  vectors  its  time  to  create  the  final 
“/motion  vector!  !  !  ! 
feasibleVel  =  smartSum(A) ; 


118 


N  Programs  for  quantitative  analysis 

70Tom  Dunbar 
y«Trident  Project 
7.27MAR 

°/0Note:  desired  simulation  must  be  changed  manually  and  desired  number  of 
“/robots  must  also  be  changed  manually  in  paracontrollerupdate  .m 

clc 

clear 

Agroupnum  =  zeros([30  1001]);  °/0pre-allocate  memory  for  all  data 
Alinktot  =  zeros ([30  1001]); 

Aetot  =  zeros ([30  1001]); 

Adegreemin  =  zeros ([30  1001]); 

Asimp  =  zeros ([1  30]); 

Aneargoal  =  zeros ([30  15]); 

Aavggroupnum  =  zeros  ([1  1001]);  #/0pre- alio  cate  memory  for  average  stuff 
7,  that  will  be  plotted 
Aavglinktot  =  zeros ([1  1001]); 

Aavgetot  =  zeros ([1  1001]); 

Adegreemin  =  zeros ([1  1001]); 


for  w  =  1:30 
w 

7»load  a  workspace 

wspace  =  [’workspace5,  num2str(w)] ; 
load(wspace) ; 

7,run  controller 

[connectedness,  neargoal]  =  fullsim(xbots,ybots,stats,gx,gy) ; 

7*run  analysis 

[groupnum,  linktot,  etot,  degreemin,  simp]  =  kconnect (connectedness) ; 
[a  b  c]  =  size (connectedness) ; 

7oSave  data  for  a  controller 
Agroupnum (w, : )  =  groupnum; 

Alinktot (w, : )  =  linktot; 

Aetot (w, : )  =  etot ; 

Adegreemin (w, : )=  degreemin; 

Asimp (w)  =  sum(simp)/c;  70percent  of  time  in  simple  redundancy 

Aneargoal (w, : )  =  neargoal; 

end 

70plotting  matrixes 

Aavggroupnum  =  sum (Agroupnum) . /30 ; 

Aavglinktot  =  sum(Alinktot) . /30 ; 

Aavgetot  =  sum (Aetot) ./30; 

Aavgdegreemin  =  sum (Adegreemin) . /30 ; 


7othe  6  measures  of  effectiveness 

Aavgneargoal  =  sum  (sum  (Aneargoal 5 )  . /15)  /30  70avgerage  time  each  robot  took 
7,  to  complete  goal,  then  average  time  through  the  swarm 
Aavgsimp  =  sum  (Asimp)  /30  7*percentage  of  time  in  simple  redundancy 
A Are agroupnum  =  sum (Aavggroupnum) ; 

AArealinktot  =  sum (Aavglinktot ) ; 

AAreaetot  =  sum(Aavgetot) ; 

AAreadegreemin  =  sum (Aavgdegreemin) ; 

70plot  everything 
figure (4) 
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hold  on; 
subplot (2,2,1) 
plot (Aavggroupnum) 
axis (  [0  1000  0  5]) 

Ts  =  [’Number  of  groups  (avg.  over  30  runs)-  Area  = 
num2str (AAreagroupnum)] ; 

xlabel( ’Time’ ) 
ylabel(’#  of  Groups’) 
title (Ts) 

subplot (2,2,2) 
plot (Aavglinktot) 
axis([0  1000  0  15]) 

Ts  =  [’Number  of  links/robot  (avg.  over  30  runs)  -  Area  =’,... 
num2str (AArealinktot)]  ; 

xlabel( ’Time’ ) 
ylabel ( ’ links/robot ’ ) 
title(Ts) 

subplot (2,2,3) 
plot (Aavgetot) 
axis ( [0  1000  0  7]) 

Ts  =  [’Algebraic  connectivity  (avg.  over  30  runs)  -  Area  =’,... 
num2str (AAreaetot)]  ; 

xlabel( ’Time’ ) 
ylabel ( ’e_2’ ) 
title (Ts) 

subplot (2,2,4) 
plot (Aavgdegreemin) 
axis ( [0  1000  0  7]) 

Ts  =  [’Minimum  degree  (avg.  over  30  runs)  -  Area  =’,... 
num2str (AAreadegreemin) ] ; 

xlabel( ’Time’ ) 
ylabel ( ’Links ’ ) 
title (Ts) 


save  goall 


function  [groupnum,  linktot,  etot,  degreemin,  simp]  =... 
kconnect (connectedness) 


'/Tom  Dunbar 
°/023  FEB 

'/Trident  Project 

°/0This  computes  the  statistics  of  a  graph  namely  K-connectedness  and  vertex 
"/degree 
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[a  b  c]  =  size (connectedness) ; 
for  cycle  =  l:c; 

adja  =  connectedness cycle) ; 

Vdegree  =  sum(adja);  '/vertex  degree 

links  =  Vdegree-1;  '/number  of  links  per  robot 

for  i  =  1 : length (Vdegree) 
h(i,i)  =  Vdegree(i); 

end 

L  =  -l*adja  +h;  '/the  laplacian  matrix 
e  =  eig(L);  '/eigienvalues  of  the  laplacian 
e  =  sort(e);  '/put  them  in  increasing  numerical  order 
groups  =  size (grouper (adja) ) ; 

groupnum( cycle)  =  groups(l);  '/number  of  groups 

linktot (cycle)  =  sum(links) /length(links) ;  '/average  number  of  links  per 
'/  robot 

etot (cycle)  =  e(2);  '/algebraic  connectivity 
degreemin(cycle)  =  min(links)  ;  '/minimum  vertex  degree 
if  min(links)  >=2 

simp  (cycle)  =  1;  '/simple  redudancy  check 

else 

simp(cycle)  =  0; 

end 

'/  if  min(links)  >=  desiredlinks 

'/  meating  =  1;  '/is  the  controller  meeting  the  minimum  number  of  links 
'/  per  robot? 

'/  else  '/only  applies  to  desiredlinks  other  than  2 

'/  meating  =0;  '/simp  will  return  the  same  info  if  desiredlinks  =2 

'/  end 

end 

'/  figure (3) 

'/  elf 
'/  hold  on; 

'/  plot  (groupnum ,  ’  k-  ’  ) 

'/  plot  (etot ,  i-i ) 

'/  plot  (ktot ,  *r-  ’ ) 

'/  plot  (linktot ,  *g-*) 

'/  axis  (  [0  100  0  15]) 


function  groups  =  grouper (adjacency) 

'/Tom  Dunbar 
'/Trident  Project 
7.17FEB 

'/this  function  takes  an  adjanecy  matrix  and  using  a  depth  first  search 
'/returns  the  groups  the  robots  are  in. 

'/start  at  the  first  robot  (first  row) 


counter  =0; 
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zeroidx  =1;  '/just  to  get  things  started 
while (length(zeroidx) ) 

unexp  =  zeroidx(l);  Stakes  the  first  new  connection  as  unexplored 
group  =  zeros  ( [1 ,  length  (adjacency)]  )  ;  "/initialize  group 
while  (length (unexp)  )  "/while  there  still  remains  unexplored 
*/  connections 

r  =  unexp (1) ; 

connex  =  f ind (adjacency (r, :)) ; 
group (r)  =2; 

for  i  =  connex  “/for  each  connection 

if  ("group (i) )  "/if  group (i)  was  previously  unconnected 
group (i)  =  1; 

end 

end 

unexp  =  find (group  ==1) ; 
end  "/end  while  loop 

counter  =  counter  +1; 

groups (counter ,  :)  =  group;  "/groups  should  only  be  2s  and  zeros 

accountability  =  sum(groups ,  1)  ;  */  sums  all  the  columns;  in  the  end 

*/  should  be  a  full  matrix  of  twos 

zeroidx  =  f ind(~accountability)  ;  "/finds  the  zeros  in  the 
*/  accountability 
end 


function  connects  =  connections (positions , stats) 

“/Tom  Dunbar 
'/21FEB 

'/Connectivity  Checker  for  the  restricted  controller,  this  takes  the 
"/positions  matrix  which  is  a  record  of  all  the  postions  and  determines 
"/which  bots  are  connected.  This  information  builds  the  connectedness 
*/  matrix 

minsep  =  10; 
maxsep  =  50; 
robonum  =  15; 

connects  =  logical (eye (robonum) ) ; 

for  r  =  1: robonum 

for  i  =  1: robonum 
if  i  ~=  r 

dist  =  sqrt ( (positions (1 , i)  -  positions (1 , r) ) ~2 .. . 

+(positions (2 , i)  -  positions (2 ,r) ) ~2) ; 


if  (dist  <  maxsep)  '/first  check  if  it  is  in  range 
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•/.Next  check  los yx/x///.y:/x/x/x/x/.rararay.y. 

oldvalue  =true; 

for  o  =1 :  length  (stats)  °/0check  each  obstacle 

A  =  [stats(o) .vx’ ;stats(o) .vy5] ; 
clear  Ab 

xB  =  positions (1 , i) ; 
yB  =  positions (2 , i) ; 

Ab(l,:)  =  A(l,:)  -  xB;  “/translating  the  obstacle  so 
'/  B  is  at  the  origin, 

Ab(2, : )  =  A(2 , : )  -  yB; 

xtrans  =  posit ions (l,r)  -xB; 
ytrans  =  positions(2,r)  -yB; 

[outside,  sanctum,  gammab,  gammat]  =. . . 

LOSconnect (xtrans , ytrans , Ab) ; 

oldvalue  =  sanctum  &  oldvalue;  “/if  any  of  these  is  a 
“/  0,  they  will  forever  be  a  zero 
end 

if  (oldvalue) 

connects (r,i)  =  true; 

end 

end  "/ends  range  check 
end"/ends  if  r  ~=i 
end 

end 


"/Tom  Dunbar 

"/Trident  project,  data  analisis 
'/13APR 

load  baselin; 
desired  =0; 


"/avg  transients 

SSlink(l)  =  sum(avglinktot (1 :400))/400; 

SSe2(l)  =  sum(avgetot(l:400))/400; 

SSmindegree (1)  =  sum(avgdegreemin(l : 400) ) /400 ; 

allgoal (1)  =  avgneargoal; 

for  desired  =  1:8 

workspae  =  [’goal’ ,num2str (desired) ] ; 
load(workspae) ; 

“/avg  transients 

SSlink(desired+l)  =  sum(Aavglinktot (1 :400) )/400; 
SSe2(desired+l)  =  sum(Aavgetot(l:400))/400; 

SSmindegree (desired+1)  =  sum(Aavgdegreemin(l :400) )/400; 

allgoal (desired+1)  =  Aavgneargoal ; 
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workspae  =  [’ comms ’ ,num2str (desired) ] ; 
load(workspae) ; 

°/«avg  transients 

Clink (desired+1)  =  sum(Aavglinktot (1 :400) )/400; 
Ce2(desired+1)  =  sum(Aavgetot (1 :400) )/400; 

Cmindegree (desired+1)  =  sum(Aavgdegreemin(l :400) )/400; 

Callgoal (desired+1)  =  Aavgneargoal ; 

end 

gainlink  =  (SSlink  -  SSlink(l))  ./SSlink(l)*100; 
gaine2  =  (SSe2  -  SSe2(l))  ./SSe2(l)*100; 
gainmindegree  =  (SSmindegree  -  SSmindegree(l) ) . . . 

. / SSmindegree ( 1 ) * 100 ; 

gaingoal  =  (allgoal  -  allgoal (1))  . /allgoal (1) *100; 

Cgainlink  =  (Clink  -  SSlink(l))  . /SSlink(l) *100 ; 

Cgaine2  =  (Ce2  -  SSe2(l))  ./SSe2(l)*100; 

Cgainmindegree  =  (Cmindegree  -  SSmindegree (1) ).. . 

. / SSmindegree ( 1 ) * 100 ; 

Cgaingoal  =  (Callgoal  -  allgoal (1))  . /allgoal (1) *100; 

Cgaingmindegree(l)  =  gainmindegree (1)  ; 

Cgainge2(l)  =  gaine2(l)  ; 

Cgaingoal (1)  =  gaingoal (1)  ; 

Cmindegree (1)  =  SSmindegree (1)  ; 

Ce2(l)  =  SSe2(l)  ; 

Callgoal (1)  =  allgoal (1)  ; 


figure (1) 
elf 

subplot (3,1,1) 
hold  on; 

plot ([0:8],  gaine2,  ’k  *’) 
plot ([0:8],  Cgaine2,  ’k  ~ ’ ) 
grid  on 

title ( ’Average  percentage  gain  in  e_2  over  baseline’) 
xlabel( ’desired  number  of  links/robot’) 
ylabel ( ’ Percentage ’ ) 
axis([0  8  -50  350]) 

subplot (3,1,2) 
hold  on; 

plot ([0:8],  gainmindegree,  ’k  *’) 
plot ([0:8],  Cgainmindegree,  ’k  ~ ’ ) 
grid  on 

title ( ’Average  percentage  gain  in  minimum  degree  over  baseline’) 
xlabel( ’desired  number  of  links/robot’) 
ylabel ( ’ Percentage ’ ) 
axis([0  8  -50  350]) 

subplot (3,1,3) 
hold  on; 
grid  on; 

plot ([0:8],  gaingoal, ’k  *’); 
plot ([0:8],  Cgaingoal, ’k  ~ ’ ) ; 
grid  on; 

title ( ’Average  percentage  gain  in  time  to  goal  over  baseline’) 
xlabel( ’desired  number  of  links/robot’) 
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ylabel ( ’ Percentage ’ ) 
axis ( [0  8  -10  60]) 


rarararammmmrm :rarararammmm%ra 


figure (2) 
elf 

[AX,H1,H2]  =  plotyy( [0:8] , SSmindegree ,  [0:8],  allgoal); 


set (HI , ’ Marker ’,’*’); 
set (HI , ’ LineStyle ’ , ’  none ’ ) ; 
get(AX(l));  70axis  one  for  k  connex 
hold  on; 

plot ([0:8] ,  SSe2, ’*’) ; 
plot ([0:8] ,  Ce2 ,  ’~’) ; 
plot  ([0:8],  Cmindegree,  ’  ~’); 
axis (  [0  8.5  0  5] ) ; 

7. 

7. 

get (AX (2)); 

70hold  on; 

plot ([0:8],  Callgoal,  ’.-’); 

70axis([0  8.5  0  500]) 

7. 

7. 

7*  7.  70Ts  =  [’Number  of  links/robot  (avg.  over  30  runs)  -  Area  =  ’, 
7*  num2str  (AArealinktot)  ]  ; 

7*  70xlabel( ’Desired  number  of  links’) 

7*  7#  ylabel  (  ’  links/robot  ’  ) 

7*  7«  7»title(Ts) 

7.  7. 

7. 

70set (get (AX(1)  ,  ’Ylabel’)  ,  ’String’ ,  ’K-Connectivity ’ ) 

7«set (get (AX (2) ,  ’Ylabel’) ,  ’String’ ,  ’Time  to  Goal’) 


function  createf igure (xl ,  yl,  y2,  y3,  y4,  y5,  y6) 
70CREATEFIGURE  (XI ,  Yl ,  Y2 ,  Y3 ,  Y4 ,  Y5 ,  Y6) 

7,  XI :  vector  of  x  data 
7,  Yl:  vector  of  y  data 
7,  Y2:  vector  of  y  data 

7,  Y3:  vector  of  y  data 

7,  Y4:  vector  of  y  data 

7,  Y5:  vector  of  y  data 

7,  Y6:  vector  of  y  data 


7,  Auto-generated  by  MATLAB  on  13-Apr-2005  21:59:24 
7,7,  Create  figure 

figurel  =  f igure (’FileName’ , ’C:\Documents  and... 

Sett ings\Administrator\Desktop\Dunbarstuf f \compareboth. png’ , ’PaperPosition . . . 
’,[-1.083  2.031  10.67  6.938]); 
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77  Create  axes 
axesl  =  axes ( . . . 

’YColor’ ,  [0  0  1]  ,  .  .  . 

’ YTick’ ,[0123455],... 

’Parent’ ,figurel) ; 
axis (axesl , [0  8.5  0  5]); 

xlabel (axesl, ’Desired  number  of  links/robot’); 
ylabel (axesl, ’K-Connectivity ’ ) ; 
grid(axesl, ’on’) ; 
hold(axesl, ’all’) ; 

77  Create  plot 
plotl  =  plot ( . . . 
xl,yl, . . . 

’LineStyle’ , ’none’ , . . . 

’Marker’ , ’*’ , . . . 

’Parent ’ , axesl) ; 

7,7  Create  plot 
plot2  =  plot ( . . . 
xl,y2, . . . 

’LineStyle’ , ’none’ , . . . 

’Marker’ , ’*’ , . . . 

’Parent ’ , axesl) ; 

77  Create  plot 
plot3  =  plot ( . . . 
xl,y3, . . . 

’LineStyle’ , ’none’ , . . . 

’Marker’ ,  . 

’Parent ’ , axesl) ; 

77  Create  plot 
plot4  =  plot ( . . . 
xl,y4, . . . 

’LineStyle’ , ’none’ , . . . 

’Marker’ 

’Parent ’ , axesl) ; 

77  Create  plot 
plot5  =  plot ( . . . 
xl,y5, . . . 

’Marker’ 

’Parent ’ , axesl) ; 

77  Create  plot 
plot6  =  plot ( . . . 
xl,y5, . . . 

’Marker’ 

’Parent ’ , axesl) ; 

77  Create  axes 
axes2  =  axes ( . . . 

’YColor’ ,  [0  0.5  0] , . . . 

’ YAxisLocation’ , ’right’ , . . . 

’YTick’ , [200  250  300  350  400  450  450],... 

’Parent’ ,figurel) ; 
axis(axes2,  [0  8.5  200  450]); 

title (axes2,{’ Comparison  of  original  and  comm,  varient  controller  ’,’with... 
respect  to  goal  completion  and  robustness  of  network’}); 

ylabel (axes2, ’Time  ot  goal  (avg.)  in  interations  of  controller’); 
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hold (axes2 , ’  all ’ ) ; 

7o°/0  Create  plot 
plot7  =  plot ( . . . 
xl,y6, . . . 

’Parent’ ,axes2, . . . 

’DisplayName ’ , ’Original’) ; 

7,7  Create  plot 
plot8  =  plot ( . . . 
xl ,y5, . . . 

’LineStyle’ ,  ’ — ’ , . . . 

’Parent’ ,axes2, . . . 

’ DisplayName ’ , ’ Comm  varient ’ ) ; 

77  Create  legend 
legendl  =  legend (... 

axes2 , { ’ Original ’ , ’ Comm  varient 
’Color’ ,  [0.8  0.8  0.8] , . . . 

’Position’ , [0.1584  0.8152  0.1338  0.06306]); 

77  Create  textbox 
annotationl  =  annotation( . . . 
figurel, ’textbox’ , . . . 

’Position’ , [0.3115  0.7132  0.1592  0.2042],... 

’FitHeightToText ’ , ’off ’ , . . . 

’String’ ,{’ Triangles  mark  upper  and’, ’lower  bounds  for’,’K-... 
connectivity ’, ’of  the  Comm  variant .’, ’While  *  mark  upper  and’ ,’ lower .. . 
bounds  of  the ’,’ Original  controller’}); 
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